#include "motion_al.h"
#include <memory>
#include <string>
#include <filesystem>
#include "config.h"
#include "zuc/motion/motionapi.h"
#include "rtdev/tio.h"
#include "rtdev/safetyboard/scbitf.h"
#include "axis/axis.h"
#include "rtbus/rtbus.h"
#include "safety/io/safeio.h"
#include "funcio/funcio.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "jkutil/iniparser.h"
#include "jkutil/threadpool.h"
#include "kine/kine.h"
#include "log/zlog.hh"
#include "rtdev/rtrobot.h"
#include "axisgroup/robot_mgr.h"
#include "zuc/ini/initraj.hh"
#include "zuc/nml_intf/zuccfg.h"    // ZUC_INIFILE
#include "zuc/nml_intf/zucglb.h"    // ZUC_INIFILE
#include "zuc/motion/usrmotintf.h"  // usrmotInit(), usrmotReadZucmotStatus(), etc.
#include "rcs_print.hh"
#include "rob_common/rob_common.h"
#include "dynamic/dynamic_cmd.h"
#include "zuc/nml_intf/zuc.hh"
#include "DYN_toppra.hh"
#include "dynamic/dynamic_cmd.h"
#include "motion/motion_cmd/motion_cmd.h"
#include "motion/axis_setting_cmd/axis_settings_cmd.h"
#include "motion/motion_cmd/rtdev_setting_cmd.h"
#include "motion/motion_cmd/moving_cmd.h"
#include "motion/motion_cmd/useless_cmd.h"
#include "motion/motion_cmd/force_control_cmd.h"
#include "zuc/ini/iniaxis.hh"
#include "zuc/ini/inijoint.hh"
#include "zuc/nml_intf/zuc_nml.hh"
#include "taskjoint.hh"
#include <float.h>  // DBL_MAX
#include "zuc/task/taskintf.h"
#include "zuc/nml_intf/zuc_nml_safe.hh"
#include "safety/safety_cmd.h"
#include "zuc/rob_common/robtool.h"
#include "zuc/rob_common/robot_payload.h"
#include "zuc/rob_common/usrframe.h"
// #include "zuc/nml_intf/zuc_nml_rob.hh"
// #include <Python.h>
// #include "pyCubic.h"
#include <eigen3/Eigen/Dense>
using namespace Eigen;
#define MAX_AXIS_INT static_cast<int>(RobJointData<double>::MAX_AXIS)

#define CRC_FILE (ZUC_USER_CONFIG_DIR "/checksum.ini")

#define ISNAN_TRAP

#ifdef ISNAN_TRAP
#define CATCH_NAN(cond)                                    \
    do {                                                   \
        if (cond)                                          \
        {                                                  \
            printf("isnan error in %s()\n", __FUNCTION__); \
            return -1;                                     \
        }                                                  \
    } while (0)
#else
#define CATCH_NAN(cond) \
    do {                \
    } while (0)
#endif
#define TCP_ERROR_TYPE 1

#define MOTAL_ERR(fmt, ...) zlog()->error("[MOTAL] {}: " fmt, __LINE__, ##__VA_ARGS__)
#define MOTAL_INFO(fmt, ...) zlog()->info("[MOTAL] " fmt, ##__VA_ARGS__)
#define MOTAL_DEBUG(fmt, ...) zlog()->debug("[MOTAL] {}: " fmt, __LINE__, ##__VA_ARGS__)

using mot::al::MotionProxy;
using mot::al::RobotALIntf;

#define DISCRETE_JOINT_LENGTH 0.25  // for toppra discrete
#define DISCRETE_CART_LENGTH 1      // for toppra discrete
#define SMALL 1.0e-4
#define MAX_NUM 1.0e8
#define CLOSE_TO_ZERO 1.0e-8
#define MAX_POINT_NUM 1001
#define MAX_BLEND_LEN 1001
#define SS_INCRE 0.1
#define REFIT_NUM 6  //motionflag 目前topp仅对6轴生效

typedef struct
{
    //设置参数
    double dh_a[MAX_AXIS_INT] = {0};  //motionflag 设置运动学参数 移到机器人类中 task层中需要
    double dh_d[MAX_AXIS_INT] = {0};
    double dh_alpha[MAX_AXIS_INT] = {0};
    double toolParameters[4] = {0};
    double install_angle[3] = {0};
    double dynParameters[MAX_AXIS_INT * 14] = {0};
    double vf_params[MAX_AXIS_INT] = {0};

} Conf_RobotProxy;

typedef struct
{
    //task status
    int task_init_finish_flag = 0;
    // axisgroup::ToolData robotToolOffset;                  // robot tool offset
    // axisgroup::ToolData robotUserOffset;                  // robot user offset
    // axisgroup::ToolData robotToolOffsets[MAX_TOOL_CNT];   // robot tool offset
    // axisgroup::ToolData robotUserOffsets[MAX_FRAME_CNT];  // robot user offset
    int usrOpeErrCode = 0;
    ZucPose utilCartPos;
    double tcpCalError[3] = {0.0};
    double cur_tc_goal_jointpos[MAX_AXIS_INT] = {0};       //当前tc段目标点的关节位置
    double cur_tc_blend_jointpos[MAX_AXIS_INT] = {0};      //当前tc段转接点的关节位置
    double cur_tc_blend_jointpos_sec[MAX_AXIS_INT] = {0};  //当前tc段转接点下一离散点的关节位置
    double tor_limit[MAX_AXIS_INT] = {0};
    int zucmotion_initialized = 0;

    zuc_traj_linearmov_t trajlinearmov;
    zuc_traj_jointmov_t trajjointmov;
    zuc_traj_movc_t trajmovc;
    zuc_traj_rigid_tap_t trajrigidtap;
    // zuc_traj_jointmov_t trajjointmov;

    enum ZUC_TASK_INTERP_ENUM interpState;  // ZUC_IDLE,READING,PAUSED,WAITING
} Status_RobotProxy;

class TOPP_member
{
public:
    BlendSegPath blend_seg_jpos;                            //这里存放从转接点到目标点的离散关节位置  motionflag topp所用队列
    double last_tc_goal_jointpos[MAX_AXIS_INT] = {0};       // for toppra: 上一个tc段目标点的关节位置         motionflag
    double last_tc_blend_jointpos[MAX_AXIS_INT] = {0};      // for toppra: 上一个tc段转接点的关节位置         motionflag
    double last_tc_blend_jointpos_sec[MAX_AXIS_INT] = {0};  //for toppra: 上一个tc段转接点下一离散点的关节位置  motionflag
    double last_tc_refit_jpos[REFIT_NUM][MAX_AXIS_INT] = {0};

    double last_tc_blend_r = 0.0;  // for toppra:上一个tc段的转接半径,如果实际运动没有转接成功，需要置零  motionflag
    double cur_tc_blend_r = 0.0;   // for toppra:当前转接段的转接半径  motionflag

    double last_final_pathp = 0.0;  //有转接的时候,转接段路径点的值在上一运动段转接点处的路径值开始进行叠加  motionflag toppuse
    int lasttc_jointNum = 0;        //上一个关节运动段运动最长的关节             motionflag toppuse
    int lasttc_left_len = 0;        //上一关节运动段从转接点到目标点离散点的个数   motionflag toppuse
    int first_command = 1;          //motionflag  topp相关
    int userset_use_toppra = 0;     //规划使用TOPP标志位
    int userset_filter_size = 0;    //motionflag topp相关
    int useradd_tor_cons = 1;       //TOPP 添加力矩限制？
    double usertor_portion = 1.0;   //topp
    int cur_blend_len;
    int cur_blend_left;
    int cur_last_blend_len;
    int blend_jpos_num;
    int jpos_len;
    double disjlen;
    double disllen;
    double disclen;
    double smooth_para;
    int robot_joint_num;
    TcPathInfo blend_path_info[1001];
    TcPathInfo path_info[1001];

    double tcp_vel_limit_toppra = 8000.0;
    int kinematic_error = 0;  //motionflag  运动学出错标志
    ConstraintsToppra cons_toppra;

    int zucTrajGetFilterSize(int filter_size);
    int zucTrajGetAddTorCons(int add_tor_cons);
    int zucTrajGetTorPortion(double tor_portion);
    int zucTrajSetDisJLen(double dislen);
    int zucTrajSetDisLLen(double dislen);
    int zucTrajSetDisCLen(double dislen);
    int zucTrajSetSmoothPara(double smooth_para);
    void get_lasttc_blend_jpos(ZucJPose* blend_jpos_for_fit);
    int movj_get_toppra_params();
    int movc_get_toppra_params();
    int get_unit_vector(double* vector, double mag, double* unit_vector);
    PmCartesian get_movc_center(PmCartesian A, PmCartesian B, PmCartesian C);
    int get_movc_planning_params(MovcStruct* circle, ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end);
    int get_movc_pos(MovcStruct* circle, const double portion, ZucPose* discrete_cartpos);
    int quaternion_interp(PmRotationMatrix* const Rot_0, PmRotationMatrix* const Rot_1, const double t, ZucPose* discrete_cartpos);
    void rot2quat(PmRotationMatrix* const m, double* q);
    int get_line_position(ZucPose* start, double* dist_unit_vector, const double len, ZucPose* line_position);
    int get_line_gesture(ZucPose* start, ZucPose* end, const double len, ZucPose* gesture);
    int rpy_quat_convert(double r, double p, double y, double* quat);
    void quartic_basic(const double t, double* B);
    int get_quat_inv(double* quat, double* quat_inv);
    int quat_quat_mult(double* quat1, double* quat2, double* quat_out);
    void power_quat(const double* q, const double t, double* Q);
    void log_quat(const double* q, double* Q);
    void exp_quat(const double* q, double* Q);
    void norm_quat(const double* q, double* q_n);
    int quat_rpy_convert(double* quat, PmRpy* const rpy);
    int ClearBlendQueue();
    int BlendEnqueue(BlendJpos element_added);
    int GetBlendFront(BlendJpos* element_front);
    int sget_blend_pathvalue_coefs_csaps(const int discrete_num, const double start_path_data, const ZucJPose* jpos_for_fit);
    // int get_pathvalue_coefs_pycubic(const int discrete_num, const ZucJPose * jpos_for_fit);
    int get_toppra_related_params(const int discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit);
    int get_blend_path_information(const int blend_jpos_len, ConstraintsToppra& constraints);
    int blend_joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int blend_joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int get_path_information(const int jpos_len, ConstraintsToppra& constraints);
    int joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int joint_acc_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    double get_min(double a, double b);
    double get_max(double a, double b);
    void init_DYN();
};

class RobotProxy : public RobotALIntf
{
public:
    RobotProxy() {}
    ~RobotProxy() {}
    int init(uint8_t grpid)
    {
        memset(&stat, 0, sizeof(stat));
        stat.rob_id = grpid;
        return 0;
    }

    int boot() override
    {
        rob.reset();

        return 0;
    }
    int shutdown()
    {
        //在调用shutdown的地方需要通知scb关闭本体电源
        //TODO:清除一些状态

        return 0;
    }
    void update() override
    {
        //等待motion层的机器人实例启动完成
        if (!rob.get())
        {
            auto _rb = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id);
            if (_rb->get_state() != axisgroup::AxisGroup::FsmState::Booting)
            {
                rob = _rb;
            }
            else
            {
                return;
            }
        }
    }

public:
    int do_enable(bool en) { return 0; }
    // int do_stop(double dec) { return 0; }
    // int do_jog_joint_cont(uint8_t id, double jvel) { return 0; }
    // int do_jog_joint_incr(uint8_t id, double step, double vel) { return 0; }
    // int do_jog_joint_abs(uint8_t id, double pos, double vel) { return 0; }
    // int do_jog_tcp_cont(uint8_t id, double tcp_vel, double ori_vel) { return 0; }
    // int do_jog_tcp_incr(uint8_t id, double step, double tcp_vel, double ori_vel) { return 0; }
    // int do_jog_tcp_abs(uint8_t id, double pos, double tcp_vel, double ori_vel) { return 0; }
    int do_jog_movj(RobJointVal jpos, MoveSpeed speed) { return 0; }
    int do_drag(bool en, uint8_t mode) { return 0; }
    // int do_movj(RobJointVal jpos, MoveSpeed speed) { return 0; }
    // int do_movl(RobTarget target, MoveSpeed speed, int tool_id = -1, int usrframe_id = -1) { return 0; }
    // int do_movc(RobTarget target, RobTarget mid, MoveSpeed speed, int tool_id = -1, int usrframe_id = -1) { return 0; }
    // int do_movs() { return 0; }
    // int do_pause(double dec) { return 0; }
    // int do_resume(double acc) { return 0; }
    int do_servo_move(bool en) { return 0; }
    int do_sync_conveyor(bool en) { return 0; }
    int do_ft_xxx() { return 0; }

    zucmot_status_t zucmotStatus;
    Conf_RobotProxy conf_robotproxy;
    Status_RobotProxy stat_robotproxy;

    int zucGetModelFromSerialNum(unsigned int serialNum, int* robotModel);
    int zucGetModelFromSerialNum();

    //负载辨识
    int zucSetPayloadIdentifyTrajStartPos(int trajId, JointTraj jointTraj, int saveOpt) override;  //motionflag 设置负载辨识轨迹起点
    int zucSetPayloadIdentifyTrajEndPos(int trajId, JointTraj jointTraj, int saveOpt) override;
    int zucStartPayloadIdentifyTraj(int trajId, JointTraj jointTraj, int withPayload) override;
    //摩擦力辨识
    int zucStopIdentifyFriction(int friction_identify_joint_index, int saveOpt) override;
    int zucStartIdentifyFriction(int friction_identify_joint_index, int saveOpt) override;

    //设置task 运动相关参数
    int config_kine_param(DHParam dhParam);
    int config_dyna_param(DynParam dynParam) override;
    int zucSetDHCom(int full_dh_com) override;
    int zucTrajSetPayload(Payload payload) override;
    int zucTrajSetPayload(Payload payload, bool saveFile = true) override;
    int zucSetRobot7dof_invflag(int inv_flag) override;
    int zucSetRobotfull_dh_flag(int full_dh_flag) override;
    int zucSetRobot7dof_arm_angle(double arm_angle) override;
    int SetRobot7dofinv_inform(int inv_flag, double arm_angle) override;
    int SetRobotFull_DH_flag(int full_dh_flag) override;
    int SetRobot7dof_kine_param_alpha_k(double k, double alpha) override;

    //转接参数设置
    int zucSetPBblendMode(int pb_blend_mode) override;
    int zucSetMovejMovelBlend(int enable) override;
    int zucSetMovejMovejBlend(int enable) override;
    int zucTrajSetTermCond(int cond, double tolerance) override;
    int zucSetupArcBlends(int arcBlendEnable,
                          int arcBlendFallbackEnable,
                          int arcBlendOptDepth,
                          int arcBlendGapCycles,
                          double arcBlendRampFreq,
                          double arcBlendTangentKinkRatio) override;

    //配置轨迹信息
    int zucTrajSetJoints(int joints) override;
    int zucTrajSetAxes(int axismask) override;
    int zucTrajSetMode(int mode) override;
    int zucTrajInit() override;
    int zucTrajEnable() override;
    int zucTrajDisable() override;
    int zucSetPlannerJerk(double joint_jerk, double carte_jerk) override;
    int zucTrajSetPlannerJerk(double joint_jerk, double carte_jerk, int saveOption) override;
    int zucSetOperationModeOption(unsigned int enable) override;

    int zucTrajDelay(double delay) override;

    //配置倍率相关参数
    int zucTrajSetScale(double scale) override;
    int zucTrajSetRapidScale(double scale) override;
    int zucTrajSetFOEnable(unsigned char mode) override;
    int zucTrajSetSOEnable(unsigned char mode) override;
    int zucTrajSetAFEnable(unsigned char enable) override;

    //安全参数设置
    double get_vel_by_stopdisttime(double jerk, int motion_type, int planner_type) override;
    int zucSetReduceSafety(ZUC_MOTION_STAT* stat, reduce_param_config reduce_param) override;
    int zucSetDragTcpSpeedLimitSetting(double dragTcpSpeedLimit) override;
    int zucSetRobotTcpVelLimit(float tcpVelLimit) override;
    int zucSetRobotStoppingDist(float stoppingDist) override;
    int zucSetRobotStoppingTime(float stoppingTime) override;
    int zucSetRobotElbowVelLimitReduce(double elbowVelLimitReduce) override;
    int zucSetDragTcpSpeedLimit(double dragTcpSpeedLimit) override;
    int zucSetRobotTcpVelLimitReduce(double tcpVelLimitReduce) override;
    int zucSetRobotMomentumLimitReduce(double momentumLimitReduce) override;
    int zucSetRobotPowerLimitReduce(double robotPowerLimitReduce) override;
    int zucSetRobotStoppingDistReduce(double stopping_dist_reduce) override;
    int zucSetRobotMomentumLimit(float momentumLimit) override;
    int zucSetRobotStoppingTimeReduce(double stopping_time_reduce) override;
    int zucSetRobotSafety(ZUC_MOTION_STAT* stat, ZUC_ROBOT_SAFETY robot_safety);

    //关节安全参数
    int zucSetLimitLinearVelocity(int enable) override;
    int zucSetLimitJointVelocity(int enable) override;

    double zucAxisGetMaxVelocity(int axis) override;
    //笛卡尔JOG相关参数设置
    int zucSetCartJogAcc(double* step_jog_pos_acc, double* step_jog_ori_acc) override;

    //传送带
    int zucTrackingConfig(int link_di, int com_cycle_num, double synch_acc, int lctsynch) override;
    int zucTrajConveyorLinearEnable(double x, double y, double z, double pulseEquivalent, int convyr_type, double max_distance) override;
    PmCartesian get_conveyor_center(PmCartesian A, PmCartesian B, PmCartesian C) override;
    int zucTrajConveyorCircularEnable(double p1x,
                                      double p1y,
                                      double p1z,
                                      double p2x,
                                      double p2y,
                                      double p2z,
                                      double p3x,
                                      double p3y,
                                      double p3z,
                                      double pulseEquivalent,
                                      int convyr_type,
                                      int rotate_tool,
                                      double max_distance) override;
    int zucTrajConveyorDisable() override;

    //产生运动的指令
    int do_jog_joint_incr(uint8_t id, int nr, double incr, double vel) override;
    int do_jog_joint_cont(uint8_t id, int nr, double vel) override;
    int do_jog_joint_abs(uint8_t id, int nr, double pos, double vel) override;
    int do_jog_tcp_cont(uint8_t id, int nr, double vel) override;
    int do_jog_tcp_incr(uint8_t id, int nr, double incr, double vel) override;
    int do_jog_tcp_abs(uint8_t id, int nr, double pos, double vel) override;
    int do_movj(RobJointVal jpos, MoveSpeed speed, MovjParam movj_param) override;
    int do_movl(RobTarget target, MoveSpeed speed, MovLParam movl_param, int tool_id, int usrframe_id) override;
    int do_movc(RobTarget target, RobTarget mid, MoveSpeed speed, Movc_Param movc_param, int tool_id, int usrframe_id) override;
    int do_movs(Movs_Param movs_param) override;
    int do_pause() override;
    int do_resume() override;
    int do_stop() override;

    int zucJogStop(int nr, int jjogmode) override;
    int zucTrajAbort();
    int zucTrajPause();
    int zucTrajStep() override;
    int zucTrajResume();
    int zucJogCont(int nr, double vel, int jjogmode);
    int zucJogIncr(int nr, double incr, double vel, int jjogmode);
    int zucJogAbs(int nr, double pos, double vel, int jjogmode);  //motionflag 关节、笛卡尔绝对值JOG
    int zucTrajLinearMove(ZucPose end,
                          int type,
                          double vel,
                          double ini_maxvel,
                          double acc,
                          double jerk,
                          double ori_vel,
                          double ori_acc,
                          int indexrotary,
                          int using_abc,
                          int di_type,
                          int di_index,
                          int di_state,
                          int planner_type);
    int zucTrajMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time);
    int zucTrajMovc(ZucPose mid,
                    ZucPose end,
                    double vel,
                    double ini_maxvel,
                    double acc,
                    double jerk,
                    int using_abc,
                    double circle_count,
                    int di_type,
                    int di_index,
                    int di_state,
                    int planner_type,
                    int circlemode);
    int zucTrajCircularMove(ZucPose end, PM_CARTESIAN center, PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double jerk)
        override;
    int zucTrajJointMove(ZucPose end,
                         int type,
                         double velPercent,
                         double ini_maxvel,
                         double acc,
                         double jerk,
                         int indexrotary,
                         int di_type,
                         int di_index,
                         int di_state,
                         int jointNum,
                         int planner_type);
    int zucJogMovl(int mvOpt, const ZucPose& posCmd, double vel, double acc, double orivel, double oriacc, double tol, int executing_line_id, int* end_cond);
    int zucJogMovj(int mvOpt, double* posCmd, double vel, double acc, double tol, int executing_line_id, int* end_cond, int is_cpos);

    //轨迹规划相关计算
    int cal_using_abc(ZucPose start_cartpos,
                      ZucPose end,
                      double vel,
                      double acc,
                      double ori_vel,
                      double ori_acc,
                      int* using_abc,
                      double* tmag_abc,
                      ZucPose trajlinearmov_pos);

    //topp 相关
    int zucTrajGetUseTopp(int use_topp) override;
    int toppraPyEnvInit() override;
    void update_related_params() override;
    void update_related_params2() override;
    void get_last_pos() override;
    int
    movl_get_toppra_params(ZucPose start_cartpos, KINEMATICS_FORWARD_FLAGS* toppra_fflags, KINEMATICS_INVERSE_FLAGS* toppra_iflags, double tmag_abc) override;
    int get_joint_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit) override;
    int get_movl_discrete_info(ZucPose start_cartpos,
                               int& discrete_num,
                               ZucJPose* jpos_for_fit,
                               ZucJPose* blend_jpos_for_fit,
                               KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                               KINEMATICS_INVERSE_FLAGS* toppra_iflags) override;
    int get_movl_discrete_info_abc(ZucPose start_cartpos,
                                   int& discrete_num,
                                   ZucJPose* jpos_for_fit,
                                   ZucJPose* blend_jpos_for_fit,
                                   KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                   KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                                   double tmag_abc) override;
    int get_movc_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit) override;
    int set_constraints_toppra(ConstraintsToppra& constraints);
    int blend_joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int blend_interpolate_discretization(const int blend_jpos_len);
    int joint_tor_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints);
    int interpolate_discretization(const int jpos_len);
    int zucMotionSetRunMode(int mode) override;
    int zucTrajRigidTap(ZucPose pos, double vel, double ini_maxvel, double acc) override;
    int zucSetTrajSmooth(int trqLimitActive, int firFltActive) override;
    int zucTrajSetRateBlendPara(int rate_blend, int topp_queue_len, int max_fw_len) override;
    int zucToppraFinishLeftTc() override;
    int zucToppraFirstCommand() override;

    //运动信息更新
    int zucMotionSetDebug(int debug) override;
    int zucMotionExitProtect(int type) override;
    int zucMotionPercentageMode(int level) override;
    int zucMotionPercentageModeLevelValue(double percentage_mode_level1_val, double percentage_mode_level2_val) override;
    int zucMotReduceMode(int reduceMode) override;

    //机器人信息更新
    int zucAdapt2Robot(unsigned int robotSerialNum) override;

    //tcp 计算相关 应该在这个类里么？
    int calcTCPErr_lea_squ(const MatrixXd& A, const VectorXd& B, const VectorXd& retTcp, double* const dist_mean, int* const num);
    int calculate_center_circle(ZucPose const* const point1, ZucPose const* const point2, ZucPose const* const point3, PmCartesian* center_cir);
    int calcTCPErr_dist(ZucPose const* const tool_to_user1,
                        ZucPose const* const tool_to_user2,
                        ZucPose const* const tool_to_user3,
                        ZucPose const* const tool_to_user4,
                        double* const dist_mean,
                        int* const num);
    int checkCalcTcpResult(const ZucPose tcpPose, const ZucPose* flangPose, size_t flangPoseCnt);
    int zucTrajCalcTcp(const unsigned char id,
                       const unsigned char saveType,
                       const unsigned char statusFlag,
                       const double* jntPos1,
                       const double* jntPos2,
                       const double* jntPos3,
                       const double* jntPos4) override;
    int zucTrajCalcTcp_six(const unsigned char id,
                           const unsigned char saveType,
                           const unsigned char statusFlag,
                           const double* jntPos1,
                           const double* jntPos2,
                           const double* jntPos3,
                           const double* jntPos4,
                           const double* jntPos5,
                           const double* jntPos6) override;
    int calcTCPErr_center_cir(ZucPose const* const tool_to_user1,
                              ZucPose const* const tool_to_user2,
                              ZucPose const* const tool_to_user3,
                              ZucPose const* const tool_to_user4,
                              double* const dist_mean,
                              int* const num);

    //user 用户坐标系计算相关
    int zucTrajCalcUser(const unsigned char id,
                        const unsigned char saveType,
                        const unsigned char statusFlag,
                        const double* jntPos1,
                        const double* jntPos2,
                        const double* jntPos3) override;

    //从类中get参数 封装成类之后这种函数应该没用了
    double zucJointGetMaxVelocity(int joint) override;
    double zucJointGetMaxAcceleration(int joint) override;
    double zucTrajGetMaxVel(void) override;
    double zucAxisGetMaxAcceleration(int axis) override;
    double zucTrajGetMaxAcceleration(void) override;
    double zucTrajGetMaxJointJerk(void) override;
    double zucTrajGetMaxCarteJerk(void) override;

    // public:  //配置具有临时性的参数
    int set_scale(double scale) { return 0; }

    // public:  // 配置参数
    int config_default_pos() { return 0; }
    int config_servo_move() { return 0; }
    int config_base_frame_id() { return 0; }
    int config_jog_param() { return 0; }
    //     int config_kine_param() { return 0; }
    //     int config_dyna_param() { return 0; }

public:
    Status& get_status() { return stat; }

    Status stat;
    std::shared_ptr<axisgroup::AxisGroup> rob;
};

MotionProxy::MotionProxy() {}
MotionProxy::~MotionProxy()
{
    for (auto& rob : robs) { rob->shutdown(); }
    robs.clear();
}

int MotionProxy::add_robot(std::shared_ptr<RobotALIntf> rob)
{
    if (rob->get_status().rob_id > robs.size())
    {
        return -1;
    }
    robs.push_back(rob);
    return 0;
}
int mot::al::MotionProxy::delete_robot(uint8_t id)
{
    if (id < 0 && id >= robs.size())
    {
        return -1;
    }
    robs.erase(robs.begin() + id);
    return 0;
}
std::shared_ptr<RobotALIntf> MotionProxy::robot(uint8_t id)
{
    if (id >= robs.size())
    {
        return nullptr;
    }
    return robs[id];
}

int MotionProxy::motion_init()
{
    int ret = motion::init_motion(zuc_cab_type);  // 初始化总线, #TODO SCB型号
    if (ret != 0)
    {
        MOTAL_ERR("motion init failed");
        return ret;
    }

    //根据配置文件创建并初始化工具坐标系
    axisgroup::RobotToolMgr::init_rob_tool(ZUC_USER_CONFIG_FILE(tool.ini));

    //根据配置文件创建并初始化用户坐标系
    axisgroup::UsrFrameMgr::init_usr_frame(ZUC_USER_CONFIG_FILE(usrframe.ini));

    //根据配置文件创建并初始化负载信息
    axisgroup::RobotPayloadMgr::init_rob_payload(ZUC_USER_CONFIG_FILE(payload.ini));

    //根据配置文件创建并初始化机器人
    Util::IniParser p(ZUC_USER_CONFIG_FILE(usersettings.ini));
    for (int i = 0;; i++)
    {
        bool has_rob = false;
        p.get("ROBOT.ROB_" + std::to_string(i), has_rob, false);
        if (!has_rob)
        {
            break;
        }
        // #1 创建机器人代理
        auto rob = std::make_shared<RobotProxy>();
        if (rob->init(i))
        {
            MOTAL_ERR("robot {} init failed", i);
            continue;
        }
        if (MotionProxy::instance().add_robot(rob))
        {
            MOTAL_ERR("add robot {} failed", i);
            continue;
        }
        MOTAL_INFO("Load robot{} success", rob->get_status().rob_id);

        // #2 创建rtdev对应的axisgroup
        auto rtrobot = std::make_shared<rtdev::RtRobot>(i);

        // #3 创建机器人轴组， 根据task层的robot，相应的添加motion层的robot
        auto robot = std::make_shared<axisgroup::Robot>();  // 无效serialnum
        if (!axisgroup::AxisGroupMgr::instance().add_axisgroup(robot, rtrobot))
        {
            rtdev::RtRobotMgr::instance().add_rtrobot(rtrobot);
            MOTAL_INFO("Add robot{} success", robot->get_group_id());
        }
        else
        {
            MOTAL_ERR("Add robot{} failed", robot->get_group_id());
            MotionProxy::instance().delete_robot(rob->get_status().rob_id);
        }
    }
    motion::start_motion();
    return 0;
}

int RobotProxy::zucGetModelFromSerialNum(unsigned int serialNum, int* robotModel)
{
    if (robotModel == nullptr)
        return -1;

    *robotModel = MODEL_UNSUPPORTED;
    if (serialNum > 710000 && serialNum < 710999)
    {
        *robotModel = MODEL_ZU_710;
    }
    else if (serialNum > 0)
    {
        *robotModel = serialNum / 10000;
    }
    return 0;
}

//motionflag 读机器人编号 逆解中会用到 重载
int RobotProxy::zucGetModelFromSerialNum()
{
    // if  (stat.config.robotModel == nullptr)
    // return -1;
    stat.config.robotModel = MODEL_UNSUPPORTED;
    if (stat.serial_num > 710000 && stat.serial_num < 710999)
    {
        stat.config.robotModel = MODEL_ZU_710;
    }
    else if (stat.serial_num > 0)
    {
        stat.config.robotModel = stat.serial_num / 10000;
    }
    return 0;
}

//motionflag 设置负载辨识轨迹起点
int RobotProxy::zucSetPayloadIdentifyTrajStartPos(int trajId, JointTraj jointTraj, int saveOpt)
{
    if (trajId < 0 || trajId > 3)
        return -1;
    std::string msg = "IdentifyTraj0 " + std::to_string(trajId) + ":start ";
    for (int i = 0; i < stat.robot_joint_num; i++) { msg += std::to_string(jointTraj.startJntPos[i]) + ", "; }
    msg + "\n";
    msg = "IdentifyTraj0" + std::to_string(trajId) + ":end ";
    for (int i = 0; i < stat.robot_joint_num; i++) { msg += std::to_string(jointTraj.startJntPos[i]) + ", "; }
    msg + "\n";
    zlog()->info(msg);

    for (int i = 0; i < stat.robot_joint_num; i++) { stat.payloadIdentifyTraj[trajId].startJntPos[i] = jointTraj.startJntPos[i]; }
    if (saveOpt)
    {
        iniSavePayloadIdentifyTrajStartPos(trajId, jointTraj, zuc_inifile);
    }

    return 0;
}

//motionflag  设置负载辨识轨迹终点
int RobotProxy::zucSetPayloadIdentifyTrajEndPos(int trajId, JointTraj jointTraj, int saveOpt)
{
    if (trajId < 0 || trajId > 3)
        return -1;

    std::string msg = "IdentifyTraj1 " + std::to_string(trajId) + ":start ";
    for (int i = 0; i < stat.robot_joint_num; i++) { msg += std::to_string(jointTraj.endJntPos[i]) + ", "; }
    msg + "\n";
    msg = "IdentifyTraj1" + std::to_string(trajId) + ":end ";
    for (int i = 0; i < stat.robot_joint_num; i++) { msg += std::to_string(jointTraj.endJntPos[i]) + ", "; }
    msg + "\n";
    zlog()->info(msg);

    for (int i = 0; i < stat.robot_joint_num; i++) { stat.payloadIdentifyTraj[trajId].endJntPos[i] = jointTraj.endJntPos[i]; }
    if (saveOpt)
    {
        iniSavePayloadIdentifyTrajEndPos(trajId, jointTraj, zuc_inifile);
    }

    return 0;
}

//motionflag 应该是开启某条运动轨迹
int RobotProxy::zucStartPayloadIdentifyTraj(int trajId, JointTraj jointTraj, int withPayload)
{
    if (fabs(conf_robotproxy.install_angle[0]) > Q_FUZZ || fabs(conf_robotproxy.install_angle[1]) > Q_FUZZ || fabs(conf_robotproxy.install_angle[2]) > Q_FUZZ)
    {
        zucOperatorError(EEROR_START_PAYLOAD_IDENTIFY, "Can't Identify Payload when not in floor-mounting");
        return -1;
    }

    auto cmd = std::make_shared<CmdStartPayloadIdentify>(stat.rob_id, withPayload, jointTraj, trajId);
    return mot::realtime_cmd_call(cmd);
}

//motionflag 看起来是停止某关节的摩擦力辨识
int RobotProxy::zucStopIdentifyFriction(int friction_identify_joint_index, int saveOpt)
{
    auto cmd = std::make_shared<CmdStopIdentifyFriction>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

//motionflag 看起来是开启某关节的摩擦力辨识
int RobotProxy::zucStartIdentifyFriction(int friction_identify_joint_index, int saveOpt)
{
    auto cmd = std::make_shared<CmdStartIdentifyFriction>(stat.rob_id, friction_identify_joint_index);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::config_kine_param(DHParam dhParam)
{
    stat.config.dhParam = dhParam;
    // setInterpDHParam (stat.config.dhParam);  // @FIXME liwang DH与7轴不符合  //ybh add  不知道这个函数干什么的
    // iniSaveDHParam (stat.motion.robot_serial_num, zucTaskDHParam, zuc_inifile);

    // zucmotCommand.dhParam = zucTaskDHParam;
    for (int i = 0; i < stat.robot_joint_num; i++)
    {
        conf_robotproxy.dh_a[i] = stat.config.dhParam.a[i] / 1000.0;
        conf_robotproxy.dh_d[i] = stat.config.dhParam.d[i] / 1000.0;
        conf_robotproxy.dh_alpha[i] = stat.config.dhParam.alpha[i] * PM_PI / 180.0;
    }
    DynPara_init_toppra(conf_robotproxy.toolParameters,
                        conf_robotproxy.install_angle,
                        conf_robotproxy.dynParameters,
                        conf_robotproxy.dh_a,
                        conf_robotproxy.dh_d,
                        conf_robotproxy.dh_alpha);
    auto cmd = std::make_shared<CmdSetDhParam>(stat.rob_id, dhParam);
    return mot::realtime_cmd_call(cmd);
    return 0;
}

int RobotProxy::zucTrajSetPayload(Payload payload)  //settingflag iniflag 设置负载并更新topp运动学
{
    // axisgroup::UsrFrame install_angle;
    // install_angle = axisgroup::UsrFrameMgr::get_usr_frame (stat.base_frame_id);
    // Payload payload = {0};  //ybh need fix
    conf_robotproxy.toolParameters[0] = payload.m;
    conf_robotproxy.toolParameters[1] = payload.p[0] / 1000.0;
    conf_robotproxy.toolParameters[2] = payload.p[1] / 1000.0;
    conf_robotproxy.toolParameters[3] = payload.p[2] / 1000.0;
    DynPara_init_toppra(conf_robotproxy.toolParameters,
                        conf_robotproxy.install_angle,
                        conf_robotproxy.dynParameters,
                        conf_robotproxy.dh_a,
                        conf_robotproxy.dh_d,
                        conf_robotproxy.dh_alpha);

    iniSavePayload(payload, zuc_inifile);
    // auto cmd = std::make_shared<CmdSetPayload> (stat.rob_id, payload);
    // return mot::realtime_cmd_call (stat.rob_id, cmd);
    return 0;
}

int RobotProxy::zucTrajSetPayload(Payload payload, bool saveFile)  //settingflag iniflag 设置负载并更新topp运动学
{
    // axisgroup::UsrFrame install_angle;
    // install_angle = axisgroup::UsrFrameMgr::get_usr_frame (stat.base_frame_id);
    // Payload payload = {0};  //ybh need fix
    conf_robotproxy.toolParameters[0] = payload.m;
    conf_robotproxy.toolParameters[1] = payload.p[0] / 1000.0;
    conf_robotproxy.toolParameters[2] = payload.p[1] / 1000.0;
    conf_robotproxy.toolParameters[3] = payload.p[2] / 1000.0;
    DynPara_init_toppra(conf_robotproxy.toolParameters,
                        conf_robotproxy.install_angle,
                        conf_robotproxy.dynParameters,
                        conf_robotproxy.dh_a,
                        conf_robotproxy.dh_d,
                        conf_robotproxy.dh_alpha);

    if (saveFile)
        iniSavePayload(payload, zuc_inifile);
    // auto cmd = std::make_shared<CmdSetPayload> (stat.rob_id, payload);
    // return mot::realtime_cmd_call (stat.rob_id, cmd);
    return 0;
}

int RobotProxy::config_dyna_param(DynParam dynParam)
{
    int i = 0;
    for (i = 0; i < 70; i++) { conf_robotproxy.dynParameters[i] = dynParam.kineticK[i]; }                                 // @FIXME liwang 需优化
    for (i = 0; i < stat.robot_joint_num; i++) { conf_robotproxy.vf_params[i] = conf_robotproxy.dynParameters[i + 42]; }  // @FIXME liwang 需优化

    DynPara_init_toppra(conf_robotproxy.toolParameters,
                        conf_robotproxy.install_angle,
                        conf_robotproxy.dynParameters,
                        conf_robotproxy.dh_a,
                        conf_robotproxy.dh_d,
                        conf_robotproxy.dh_alpha);  // @FIXME liwang DH与7轴不符合
    auto cmd = std::make_shared<CmdSetDynParam>(stat.rob_id, dynParam);
    return mot::realtime_cmd_call(cmd);
}

double RobotProxy::zucJointGetMaxVelocity(int joint)  //motionflag getflag 获取关节速度上限 没地方用  以后用类封装的话该参数应该能直接从类中读取
{
    if (joint < 0 || joint >= MAX_AXIS_INT)
    {
        return 0;
    }
    return stat.robot_config.joint_limit.vel[joint];
}

double RobotProxy::zucJointGetMaxAcceleration(int joint)  //motionflag  获取关节加速度上限 解析时关节运动会用到
{
    if (joint < 0 || joint >= MAX_AXIS_INT)
    {
        return 0;
    }
    return stat.robot_config.joint_limit.acc[joint];
}

/*! functions involving carthesian Axes (X,Y,Z,A,B,C,U,V,W) */
double RobotProxy::zucAxisGetMaxVelocity(int axis)  //motionflag 获取笛卡尔轴的最大速度、影响笛卡尔运动
{
    if (axis < 0 || axis >= ZUCMOT_MAX_AXIS)
    {
        return 0;
    }
    if (axis < 3)
    {
        return stat.robot_config.traj.vel.trans;
    }
    else
    {
        return stat.robot_config.traj.vel.ori;
    }
}

//added by alex.li at 2018.08.02 for using traj.maxaccel
double RobotProxy::zucTrajGetMaxAcceleration(void) { return stat.robot_config.traj.acc.trans; }  //motionflag 获取轨迹最大加速度

//added by alex.li at 2018.08.02 for using traj.maxvel
double RobotProxy::zucTrajGetMaxVel(void)  //motionflag 获取轨迹最大速度
{
    return stat.robot_config.traj.vel.trans;
}

double RobotProxy::zucAxisGetMaxAcceleration(int axis)  //motionflag  获取笛卡尔轴最大加速度
{
    if (axis < 0 || axis >= ZUCMOT_MAX_AXIS)
    {
        return 0;
    }
    if (axis < 3)
    {
        return stat.robot_config.traj.acc.trans;
    }
    else
    {
        return stat.robot_config.traj.acc.ori;
    }
}

int RobotProxy::zucJogCont(int nr, double vel, int jjogmode)  //motionflag 笛卡尔、关节连续JOG
{
    if (stat.protective_stop == 1)
    {
        rcs_print("Cannot do JOG when in protective_stop status\n");
        return -1;
    }

    int joint = -1;
    int axis = -1;
    if (jjogmode == JOGJOINT)
    {
        if (nr < 0 || nr >= MAX_AXIS_INT)
        {
            return 0;
        }
        if (vel > stat.config.JointConfig[nr].MaxVel)
        {
            vel = stat.config.JointConfig[nr].MaxVel;
        }
        else if (vel < -stat.config.JointConfig[nr].MaxVel)
        {
            vel = -stat.config.JointConfig[nr].MaxVel;
        }
        joint = nr;
        axis = -1;  //NA
    }
    else
    {
        if (nr < 0 || nr >= ZUCMOT_MAX_AXIS)
        {
            return 0;
        }
        if (vel > stat.config.AxisConfig[nr].MaxVel)
        {
            vel = stat.config.AxisConfig[nr].MaxVel;
        }
        else if (vel < -stat.config.AxisConfig[nr].MaxVel)
        {
            vel = -stat.config.AxisConfig[nr].MaxVel;
        }
        joint = -1;  //NA
        axis = nr;
    }

    auto cmd = std::make_shared<CmdJogCont>(stat.rob_id, joint, axis, vel);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucJogIncr(int nr, double incr, double vel, int jjogmode)  //motionflag 笛卡尔、关节增量JOG
{
    if (stat.protective_stop == 1)
    {
        rcs_print("Cannot do JOG when in protective_stop status\n");
        return -1;
    }

    int joint = -1;
    int axis = -1;

    if (jjogmode == JOGJOINT)
    {
        if (nr < 0 || nr >= MAX_AXIS_INT)
        {
            return 0;
        }
        if (vel > stat.config.JointConfig[nr].MaxVel)
        {
            vel = stat.config.JointConfig[nr].MaxVel;
        }
        else if (vel < -stat.config.JointConfig[nr].MaxVel)
        {
            vel = -stat.config.JointConfig[nr].MaxVel;
        }
        joint = nr;
        axis = -1;  //NA
    }
    else
    {
        if (nr < 0 || nr >= ZUCMOT_MAX_AXIS)
        {
            return 0;
        }
        if (vel > stat.config.AxisConfig[nr].MaxVel)
        {
            vel = stat.config.AxisConfig[nr].MaxVel;
        }
        else if (vel < -stat.config.AxisConfig[nr].MaxVel)
        {
            vel = -stat.config.AxisConfig[nr].MaxVel;
        }
        joint = -1;  //NA
        axis = nr;
    }

    auto cmd = std::make_shared<CmdJogIncr>(stat.rob_id, joint, axis, vel, incr);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucJogAbs(int nr, double pos, double vel, int jjogmode)
{
    if (stat.protective_stop == 1)
    {
        rcs_print("Cannot do JOG when in protective_stop status\n");
        return -1;
    }
    int joint = -1;
    int axis = -1;
    if (jjogmode == JOGJOINT)
    {
        if (nr < 0 || nr >= MAX_AXIS_INT)
        {
            return 0;
        }
        joint = nr;
        axis = -1;  //NA
    }
    else
    {
        if (nr < 0 || nr >= ZUCMOT_MAX_AXIS)
        {
            return 0;
        }
        joint = -1;  //NA
        axis = nr;
    }
    auto cmd = std::make_shared<CmdJogAbs>(stat.rob_id, joint, axis, vel, pos);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::do_jog_joint_incr(uint8_t id, int nr, double incr, double vel)
{
    int jjogmode = JOGJOINT;
    stat.rob_id = id;
    zucJogIncr(nr, incr, vel, jjogmode);
    return 0;
}

int RobotProxy::do_jog_joint_cont(uint8_t id, int nr, double vel)
{
    int jjogmode = JOGJOINT;
    stat.rob_id = id;
    int ret = zucJogCont(nr, vel, jjogmode);
    return ret;
}

int RobotProxy::do_jog_joint_abs(uint8_t id, int nr, double pos, double vel)
{
    int jjogmode = JOGJOINT;
    stat.rob_id = id;
    int ret = zucJogAbs(nr, pos, vel, jjogmode);
    return ret;
}

int RobotProxy::do_jog_tcp_cont(uint8_t id, int nr, double vel)
{
    int jjogmode = JOGTELEOP;
    stat.rob_id = id;
    int ret = zucJogCont(nr, vel, jjogmode);
    return ret;
}

int RobotProxy::do_jog_tcp_incr(uint8_t id, int nr, double incr, double vel)
{
    int jjogmode = JOGTELEOP;
    stat.rob_id = id;
    int ret = zucJogCont(nr, vel, jjogmode);
    return ret;
}

int RobotProxy::do_jog_tcp_abs(uint8_t id, int nr, double pos, double vel)
{
    int jjogmode = JOGTELEOP;
    stat.rob_id = id;
    int ret = zucJogAbs(nr, pos, vel, jjogmode);
    return ret;
}

int RobotProxy::zucJogStop(int nr, int jjogmode)
{
    int joint = -1;
    int axis = -1;
    if (jjogmode == JOGJOINT)
    {
        if (nr < 0 || nr >= MAX_AXIS_INT)
        {
            return 0;
        }
        joint = nr;
        axis = -1;  //NA
        // zucmotCommand.jog.joint = nr;
        // zucmotCommand.jog.axis = -1; //NA
    }
    else
    {
        if (nr < 0 || nr >= ZUCMOT_MAX_AXIS)
        {
            return 0;
        }
        joint = -1;  //NA
        axis = nr;
        // zucmotCommand.jog.joint = -1; //NA
        // zucmotCommand.jog.axis = nr;
    }
    // auto cmd = std::make_shared<CmdJogStop> (stat.rob_id, joint, axis);
    // return mot::realtime_cmd_call (stat.rob_id, cmd);
    return 0;
}

int RobotProxy::zucTrajSetJoints(int joints)  //settingflag motionflag 设置机器人关节个数
{
    if (joints <= 0 || joints > MAX_AXIS_INT)
    {
        rcs_print("zucTrajSetJoints failing: joints=%d\n", joints);
        return -1;
    }
    stat.config.TrajConfig.Joints = joints;
    stat.robot_joint_num = joints;
    TOPP.robot_joint_num = joints;
    auto cmd = std::make_shared<CmdSetNumJoints>(stat.rob_id, joints);
    int retval = mot::realtime_cmd_call(cmd);

    if (zuc_debug & ZUC_DEBUG_CONFIG)
    {
        rcs_print("%s(%d) returned %d\n", __FUNCTION__, joints, retval);
    }
    return retval;
}

int RobotProxy::zucTrajSetAxes(int axismask)  //settingflag configflag 设置包含的轴组
{
    int axes = 0;
    for (int i = 0; i < ZUCMOT_MAX_AXIS; i++)
        if (axismask & (1 << i))
            axes = i + 1;

    stat.config.TrajConfig.DeprecatedAxes = axes;
    stat.config.TrajConfig.AxisMask = axismask;

    if (zuc_debug & ZUC_DEBUG_CONFIG)
    {
        rcs_print("%s(%d, %d)\n", __FUNCTION__, axes, axismask);
    }
    return 0;
}

int RobotProxy::zucTrajSetMode(int mode)  //motionflag 设置运动模式
{
    if (stat.protective_stop == 1 && mode == ZUC_TRAJ_MODE_DRAG)
    {
        auto cmd = std::make_shared<CmdExitProtect>(stat.rob_id, 0);
        return mot::realtime_cmd_call(cmd);
    }

    if (stat.protective_stop == 1 && mode != ZUC_TRAJ_MODE_FREE)
    {
        rcs_print("Cannot change to non-free mode when in protective_stop status\n");
        return -1;
    }

    switch (mode)
    {
    case ZUC_TRAJ_MODE_FREE: {
        auto cmd = std::make_shared<CmdFree>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_COORD: {
        auto cmd = std::make_shared<CmdCoord>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_TELEOP: {
        auto cmd = std::make_shared<CmdTeleop>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_TELEOP_TOOL: {
        auto cmd = std::make_shared<CmdTeleopTool>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_SERVOJOP: {
        // auto cmd = std::make_shared<CmdServojop>(stat.rob_id,
        //                                          );
        // return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_ADMITTANCE: {
        auto cmd = std::make_shared<CmdAdmittance>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_DRAG: {
        auto cmd = std::make_shared<CmdDrag>(stat.rob_id);
        return mot::realtime_cmd_call(cmd);
    }

    default:
        return -1;
    }
    return 0;
}

double RobotProxy::zucTrajGetMaxJointJerk(void) { return stat.robot_config.joint_limit.jerk; }  //motionflag  获取关节加加速度  用在解析关节运动

double RobotProxy::zucTrajGetMaxCarteJerk(void) { return stat.robot_config.robot_model.traj.jerk.trans; }  //motionflag  获取笛卡尔加加速度 用在解析笛卡尔运动

int RobotProxy::zucTrajSetScale(double scale)  //motionflag 更新倍率 但是真正的倍率应该是 net_feed_scale
{
    // 意义不明
    if (scale < 0.0)
    {
        scale = 0.0;
    }

    auto cmd = std::make_shared<CmdFeedScale>(stat.rob_id, scale);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajSetRapidScale(double scale)  //motionflag 更新倍率 但是真正的倍率应该是 net_feed_scale
{
    if (scale < 0.0)
    {
        scale = 0.0;
    }

    auto cmd = std::make_shared<CmdRapidScale>(stat.rob_id, scale);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajSetFOEnable(unsigned char mode)  //motionflag 好像是各种倍率模式的开关 看起来没用到
{
    auto cmd = std::make_shared<CmdFsEnable>(stat.rob_id, mode);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajSetSOEnable(unsigned char mode)  //motionflag 好像是各种倍率模式的开关 看起来没用到
{
    auto cmd = std::make_shared<CmdSsEnable>(stat.rob_id, mode);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajSetAFEnable(unsigned char enable)  //motionflag 好像是各种倍率模式的开关 看起来没用到
{
    auto cmd = std::make_shared<CmdAfEnable>(stat.rob_id, enable);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajGetUseTopp(int use_topp)
{
    stat.userset_use_toppra = use_topp;
    if (stat.unable_use_toppra)
    {
        stat.userset_use_toppra = 0;
        MOTAL_INFO(" pyimport failed --- unable_use_toppra \n");
    }
    if (/* use_topp &&  */ TOPP.blend_seg_jpos.jpos == NULL)
    {
        MOTAL_INFO("[zucTrajGetUseTopp]  ---> start malloc blend_seg_jpos space\n");
        TOPP.blend_seg_jpos.jpos = (BlendJpos*)malloc(sizeof(BlendJpos) * MAX_BLEND_LEN);
        if (TOPP.blend_seg_jpos.jpos == NULL)
        {
            MOTAL_INFO("[zucTrajGetUseTopp]  ---> failed malloc blend_seg_jpos space\n");
        }
        else
        {
            TOPP.blend_seg_jpos.rear = 0;
            TOPP.blend_seg_jpos.front = 0;
            MOTAL_INFO("[zucTrajGetUseTopp]  ---> succeed malloc blend_seg_jpos space\n");
        }
    }
    MOTAL_INFO(" ************************************************** use_topp = %d ************************************************** \n",
               stat.userset_use_toppra);
    return 0;
}

int RobotProxy::toppraPyEnvInit()  //motionflag topprapython 相关
{
#ifdef EN_TOPPRA
    MOTAL_INFO(" #######################################################========================================================== Py_Initialize "
               "#######################################################==========================================================\n");
    if (PyImport_AppendInittab("pyCubic", initpyCubic) == -1)
    {
        MOTAL_INFO("Error: could not extend in-built modules table\n");  //增加一个参数设置，不能使用toppra算法
        stat.unable_use_toppra = 1;
    }
    else
    {
        stat.unable_use_toppra = 0;
    }
    Py_Initialize();
    // pmodule = PyImport_ImportModule("pyCubic");    //disableflag 先注释
    // if (!pmodule)
    // {
    //     stat.unable_use_toppra = 1;
    //     MOTAL_INFO("Error: !pmodule\n");  //增加一个参数设置，不能使用toppra算法
    // }
    // else
    // {
    //     stat.unable_use_toppra = 0;
    // }
    return 0;
#else
    stat.unable_use_toppra = 1;
    return -1;
#endif
}

int RobotProxy::zucTrajInit()  //motionflag 配置轨迹信息
{
    toppraPyEnvInit();

    int retval = 0;
    // RobotModelData robotModelData;
    stat.config.TrajConfig.Inited = 0;
    stat.config.TrajConfig.Joints = 0;
    stat.config.TrajConfig.MaxAccel = DBL_MAX;
    stat.config.TrajConfig.DeprecatedAxes = 0;
    stat.config.TrajConfig.AxisMask = 0;
    stat.config.TrajConfig.LinearUnits = 1.0;
    stat.config.TrajConfig.AngularUnits = 1.0;
    stat.config.TrajConfig.MotionId = 0;
    stat.config.TrajConfig.MaxVel = DEFAULT_TRAJ_MAX_VELOCITY;

    // init zucmot interface
    stat.config.TrajConfig.Inited = 1;

    // initialize parameters from ini file

    // // @FIXME 与initOnekeyset函数作用相同，可以整合为一个函数
    // if (0 != zucInitAutoOp(zuc_inifile))
    // {
    //     MOTAL_INFO("iniAutoSetting failed\n\n");
    //     retval = -1;
    // }
    // else
    // {
    //     MOTAL_INFO("***************iniAutoSetting successed***************\n\n");
    //     // MOTAL_INFO("***************zucStatus->autoopsetting.autoenable = %d***************\n\n", zucStatus->autoopsetting.onekeyenable);
    //     // MOTAL_INFO("***************zucStatus->autoopsetting.autoprogram = %d***************\n\n", zucStatus->autoopsetting.autoprogram);
    // }

    /*       //diabaleflag 先注释 ybh need fix
    // 优先读取机型文件，确认机器人轴数量
    if (0 != zucInitRobot(zuc_inifile))
    {
        rcs_print("KineDH init failed!\n");
        retval = -1;
    }

    // 与机器人轴数相关的配置读取
    if (0 != initSetAboutJoint(zuc_inifile, stat.robot_joint_num))
    {
        MOTAL_INFO("initSetAboutJoint failed\n\n");
        retval = -1;
    }

    //初始化并加载关节统计数据
    ServoStatistic::get()->init(zucStatus->motion.robot_serial_num);
    // ZucIniFile _trajInifile;
    // _trajInifile.Open(zuc_inifile);
    Util::IniParser _ini_file(zuc_inifile);
    ServoStatistic::get()->load(&_ini_file);

    if (0 != zucInitFuncIOMap(zuc_inifile))
    {
        rcs_print("FuncIO init failed!\n");
        retval = -1;
    }

    if (zuc_cab_type >= 2)
    {
        zucInitSafetyFuncIOSettings(zuc_inifile);
        zucInitSafetyRobotPowerLimitSettings(zuc_inifile);
    }

    zucInitTIOSetup(zuc_inifile);

    // 传感器硬件相关设置
    zucInitTorqSensorSetup(zuc_inifile);

    zucInitRobotSafetyLimitSettings(zuc_inifile);
    zucInitRobotReduceSafetyLimitSettings(zuc_inifile);
    zucInitNetExceptionHandle(zuc_inifile);
    zucInitDragSpeedLimitSettings(zuc_inifile);
    */
    return retval;
}

int RobotProxy::zucTrajEnable()  //motionflag 激活轨迹
{
    if (stat.protective_stop == 1)
    {
        rcs_print("Cannot enable traj when in protective_stop status\n");
        return -1;
    }

    auto cmd = std::make_shared<CmdEnable>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajDisable()  //motionflag 关闭轨迹
{
    ServoStatistic::get()->write2File(zuc_inifile);
    auto cmd = std::make_shared<CmdDisable>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajAbort()  //motionflag  中断轨迹
{
    auto cmd = std::make_shared<CmdAbort>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajPause()  //motionflag 轨迹暂停
{
    auto cmd = std::make_shared<CmdPause>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajStep()  //motionflag
{
    auto cmd = std::make_shared<CmdStep>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajResume()  //motionflag
{
    auto cmd = std::make_shared<CmdResume>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajDelay(double delay)  //没用
{
    /* nothing need be done here - it's done in task controller */

    return 0;
}

int RobotProxy::do_pause()
{
    int ret = zucTrajPause();
    return ret;
}

int RobotProxy::do_resume()
{
    int ret = zucTrajResume();
    return ret;
}

int RobotProxy::zucTrackingConfig(int link_di, int com_cycle_num, double synch_acc, int lctsynch)  //motionflag 传送带相关
{
    auto cmd = std::make_shared<CmdSetTrackingConfig>(stat.rob_id, link_di, lctsynch, synch_acc, com_cycle_num);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetDHCom(int full_dh_com)  //motionflag 设置全参数补偿
{
    // setFullDHCom(full_dh_com);
    stat.motion.full_dh_com = full_dh_com;

    auto cmd = std::make_shared<CmdSetFullDhCom>(stat.rob_id, full_dh_com);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetLimitLinearVelocity(int enable)  //motionflag safeflag 设置线速度
{
    auto cmd = std::make_shared<CmdSetEnableLimitLinearVelocity>(stat.rob_id, enable);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetLimitJointVelocity(int enable)  //motionflag safeflag 设置关节速度
{
    auto cmd = std::make_shared<CmdSetEnableLimitJointVelocity>(stat.rob_id, enable);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetPlannerJerk(double joint_jerk, double carte_jerk)  //配置S规划
{
    auto cmd = std::make_shared<CmdSetPlannerJerk>(stat.rob_id, joint_jerk, carte_jerk);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetPBblendMode(int pb_blend_mode)  //motionflag 设置转接模式为PB转接
{
    auto cmd = std::make_shared<CmdSetPbblendmode>(stat.rob_id, pb_blend_mode);
    return mot::realtime_cmd_call(cmd);
}

//Send blend flag (int enable) to the controller
int RobotProxy::zucSetMovejMovelBlend(int enable)  //motionflag 设置关节与直线之间可转接
{
    auto cmd = std::make_shared<CmdSetEnableMovejmovelblend>(stat.rob_id, enable);
    return mot::realtime_cmd_call(cmd);
}

//Send blend flag (int enable) to the controller
int RobotProxy::zucSetMovejMovejBlend(int enable)  //motionflag 设置关节与关节之间可转接
{
    auto cmd = std::make_shared<CmdSetEnableMovejmovejblend>(stat.rob_id, enable);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetCartJogAcc(double* step_jog_pos_acc, double* step_jog_ori_acc)  //motionflag 设置笛卡尔JOG时的加速度
{
    auto cmd = std::make_shared<CmdSetCartJogAcc>(stat.rob_id, step_jog_ori_acc, step_jog_pos_acc);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajSetPlannerJerk(double joint_jerk, double carte_jerk, int saveOption)  //motionflag 设置加加速度 iniflag
{
    if (saveOption)
    {
        iniSavePlannerJerk(joint_jerk, carte_jerk, zuc_inifile);
    }
    double jerk1, jerk2;
    jerk1 = joint_jerk;
    jerk2 = carte_jerk;

    zucSetPlannerJerk(jerk1, jerk2);
    return 0;
}

int RobotProxy::zucTrajConveyorLinearEnable(double x,
                                            double y,
                                            double z,
                                            double pulseEquivalent,
                                            int convyr_type,
                                            double max_distance)  //motionflag  传送带相关
{
    //  MOTAL_INFO("----------zucTrajConveyorLinearEnable----------:  x = %f y=%f z=%f unit=%f   \n",x,y,z,pulseEquivalent);  //disableflag 先注释 ybh need fix
    // zucmotCommand.command = ZUCMOT_TRACKING_ENABLE;
    // zucmotCommand.enaconvyrtrack.convyr_type = convyr_type;
    // zucmotCommand.enaconvyrtrack.conveyorDir.x = x;
    // zucmotCommand.enaconvyrtrack.conveyorDir.y = y;
    // zucmotCommand.enaconvyrtrack.conveyorDir.z = z;
    // zucmotCommand.enaconvyrtrack.conveyorPulseEquivalent = pulseEquivalent;
    // zucmotCommand.enaconvyrtrack.max_distance = max_distance;
    // if (fabs(x) < 1e-8 && fabs(y) < 1e-8 && fabs(z) < 1e-8)
    // {
    //     zucOperatorError(ERROR_NO_TRACKING_DIR, "linear conveyor tracking direction is zero");
    //     return -1;
    // }
    // return usrmotWriteZucmotCommand(&zucmotCommand);
    return 0;
}

PmCartesian RobotProxy::get_conveyor_center(PmCartesian A, PmCartesian B, PmCartesian C)  //motionflag 传送带相关计算
{
    PmCartesian u, v;    //求出的新坐标系X,Y方向上的向量
    PmCartesian center;  //圆心
    //PmCartesian AB, AC;  // AB: 向量AB， AC：向量AC
    PmCartesian ex, ey;  //ex: 新坐标系X轴单位向量 ，ey: 新坐标系Y轴单位向量
    PmCartesian vy, vx;  // C点做垂先和AB相交，所得的X轴上的向量。
    double xB, xC, yC;   // AB的长度。

    PmCartesian tempEx, tempEy;
    //计算 u = B - A;
    pmCartCartSub(&B, &A, &u);  //计算 v = C - A;
    pmCartCartSub(&C, &A, &v);  //计算 xB = norm(u)
    pmCartMag(&u, &xB);         //计算 ex = u/xB
    pmCartUnit(&u, &ex);        //vx = （v * ex）*ex;
    pmCartCartDot(&v, &ex, &xC);
    pmCartScalMult(&ex, xC, &vx);  //计算 vy = v - vx;
    pmCartCartSub(&v, &vx, &vy);   //计算Y轴单位向量 ey = ey/|ey|
    pmCartUnit(&vy, &ey);          //计算 yC = |vy|
    pmCartMag(&vy, &yC);           //计算center = A + 1/2 xB*ex + (-xB (xB - xC) xC + xB yC^2)/(2 xB yC)*ey
    pmCartScalMult(&ex, 0.5 * xB, &tempEx);
    pmCartCartAdd(&A, &tempEx, &tempEx);
    pmCartScalMult(&ey, (-1 * xB * (xB - xC) * xC + xB * yC * yC) / (2 * xB * yC), &tempEy);
    pmCartCartAdd(&tempEx, &tempEy, &center);

    return center;
}

int RobotProxy::zucTrajConveyorCircularEnable(double p1x,
                                              double p1y,
                                              double p1z,
                                              double p2x,
                                              double p2y,
                                              double p2z,
                                              double p3x,
                                              double p3y,
                                              double p3z,
                                              double pulseEquivalent,
                                              int convyr_type,
                                              int rotate_tool,
                                              double max_distance)  //motionflag 传送带相关
{
    // MOTAL_INFO("enter zucTrajConveyorCircularEnable\n");
    //根据给定三点坐标(当前用户坐标系下获得)计算传送带圆心坐标系在当前用户坐标系下的变换矩阵
    PmCartesian pos1, pos2, pos3, z0, x0, y0;
    //double theta;
    pos1.x = p1x;
    pos1.y = p1y;
    pos1.z = p1z;
    pos2.x = p2x;
    pos2.y = p2y;
    pos2.z = p2z;
    pos3.x = p3x;
    pos3.y = p3y;
    pos3.z = p3z;

    double sm[3] = {0.0};
    double me[3] = {0.0};
    double se[3] = {0.0};
    double ms_norm = 0.0, me_norm = 0.0, se_norm = 0.0;
    sm[0] = pos1.x - pos2.x;
    sm[1] = pos1.y - pos2.y;
    sm[2] = pos1.z - pos2.z;
    me[0] = pos3.x - pos2.x;
    me[1] = pos3.y - pos2.y;
    me[2] = pos3.z - pos2.z;
    se[0] = -pos1.x + pos3.x;
    se[1] = -pos1.y + pos3.y;
    se[2] = -pos1.z + pos3.z;
    ms_norm = sqrt(sm[0] * sm[0] + sm[1] * sm[1] + sm[2] * sm[2]);
    me_norm = sqrt(me[0] * me[0] + me[1] * me[1] + me[2] * me[2]);
    se_norm = sqrt(se[0] * se[0] + se[1] * se[1] + se[2] * se[2]);

    double area = 0.5 * (ms_norm + me_norm + se_norm);
    if (area * (area - ms_norm) * (area - me_norm) * (area - se_norm) < 1e-5)
    {
        zucOperatorError(ERROR_P1P2P3_COLINEAR, "ERROR: circular conveyor P1, P2 and P3 are colinear");
        return -1;
    }
    /* 标定方法1： 如果标定的是传送带上共圆的三点, p1, p2, p3确定传送带圆心坐标o(p1, p2, p3一定是在世界坐标系下的坐标, 圆心o也是世界坐标系下的坐标), (op1)坐标系+x, (op1)叉乘(op2)是轴线方向即+z,(+z)叉乘(op1)是+y*/
    PmCartesian center;
    center = get_conveyor_center(pos1, pos2, pos3);
    pmCartCartSub(&pos1, &center, &x0);
    pmCartCartSub(&pos3, &center, &y0);
    pmCartUnitEq(&x0);
    pmCartUnitEq(&y0);
    pmCartCartCross(&x0, &y0, &z0);
    pmCartUnitEq(&z0);
    pmCartCartCross(&z0, &x0, &y0);
    /*  // disableflag 
    zucmotCommand.enaconvyrtrack.o_to_world.tran.x = center.x;
    zucmotCommand.enaconvyrtrack.o_to_world.tran.y = center.y;
    zucmotCommand.enaconvyrtrack.o_to_world.tran.z = center.z;*/  //disableflag ybh need fix

    // MOTAL_INFO("position of o_conveyor are: %f %f %f\n", center.x, center.y, center.z);

    /* 标定方法2： 如果标定的是传送带轴线方向, p1是圆心, p1p2方向是轴线方向, (p1p2)叉乘(p2p3)是坐标系+y, (+y)叉乘(p1p2)是坐标系+x */
    // pmCartCartSub(&pos2, &pos1, &z0);
    // pmCartUnitEq(&z0);
    // pmCartCartSub(&pos3, &pos2, &x0);
    // pmCartUnitEq(&x0);
    // pmCartCartDot(&z0,&x0,&theta);

    // if((1-theta)<0.001)
    // {
    //     MOTAL_INFO("Circle conveyor failed ---> inappropriate points! \n");
    //     return -1;
    // }
    // pmCartCartCross(&z0, &x0,&y0);
    // pmCartUnitEq(&y0);
    // pmCartCartCross(&y0, &z0,&x0);
    // pmCartUnitEq(&x0);
    // MOTAL_INFO("[zucTrajConveyorCircularEnable] ---> x0 = %f %f %f, y0 = %f %f %f, z0 = %f %f %f\n", x0.x, x0.y, x0.z, y0.x, y0.y, y0.z, z0.x, z0.y, z0.z);
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.x = p1x;
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.y = p1y;
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.z = p1z;

    /* //disableflag 
    zucmotCommand.enaconvyrtrack.o_to_world.rot.x = x0;
    zucmotCommand.enaconvyrtrack.o_to_world.rot.y = y0;
    zucmotCommand.enaconvyrtrack.o_to_world.rot.z = z0;

    zucmotCommand.enaconvyrtrack.convyr_type = convyr_type;
    zucmotCommand.enaconvyrtrack.conveyorPulseEquivalent = pulseEquivalent;
    zucmotCommand.enaconvyrtrack.rotate_tool = rotate_tool;
    zucmotCommand.enaconvyrtrack.max_distance = max_distance;
    zucmotCommand.command = ZUCMOT_TRACKING_ENABLE;
    return usrmotWriteZucmotCommand(&zucmotCommand);*/   //disableflag  ybh need fix
    return 0;
}

int RobotProxy::zucTrajConveyorDisable()  //motionflag 传送带参数
{
    // MOTAL_INFO("----------zucTrajConveyorDisable----------:    \n");
    auto cmd = std::make_shared<CmdTrackingDisable>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::checkCalcTcpResult(const ZucPose tcpPose, const ZucPose* flangPose, size_t flangPoseCnt)  //safeflag tcp检查
{
    //tcp的长度必须在一定范围内
    double tcpLen = sqrt(tcpPose.tran.x * tcpPose.tran.x + tcpPose.tran.y * tcpPose.tran.y + tcpPose.tran.z * tcpPose.tran.z);
    if (tcpLen > 2000.0)
    {
        MOTAL_INFO("tcp calibration position is over workspace limit\n");
        return -1;
    }
    //tcp的误差要在一定范围内
    //TODO:
    return 0;
}

int RobotProxy::calculate_center_circle(ZucPose const* const point1,
                                        ZucPose const* const point2,
                                        ZucPose const* const point3,
                                        PmCartesian* center_cir)  //计算圆心
{
    if (!point1 || !point2 || !point3)
        return -1;
    double collinear =
        fabs((point3->tran.y - point1->tran.y) * (point2->tran.x - point1->tran.x) - (point2->tran.y - point1->tran.y) * (point3->tran.x - point1->tran.x));
    if (collinear < TP_POS_EPSILON)
    {
        MOTAL_INFO("collinear is %f\n", collinear);

        return -1;
    }
    double x1 = point1->tran.x;
    double y1 = point1->tran.y;
    double z1 = point1->tran.z;
    double x2 = point2->tran.x;
    double y2 = point2->tran.y;
    double z2 = point2->tran.z;
    double x3 = point3->tran.x;
    double y3 = point3->tran.y;
    double z3 = point3->tran.z;

    double a1 = y1 * z2 - y2 * z1 - y1 * z3 + y3 * z1 + y2 * z3 - y3 * z2;
    double b1 = -(x1 * z2 - x2 * z1 - x1 * z3 + x3 * z1 + x2 * z3 - x3 * z2);
    double c1 = x1 * y2 - x2 * y1 - x1 * y3 + x3 * y1 + x2 * y3 - x3 * y2;
    double d1 = -(x1 * y2 * z3 - x1 * y3 * z2 - x2 * y1 * z3 + x2 * y3 * z1 + x3 * y1 * z2 - x3 * y2 * z1);
    double a2 = 2 * (x2 - x1);
    double b2 = 2 * (y2 - y1);
    double c2 = 2 * (z2 - z1);
    double d2 = pmSq(x1) + pmSq(y1) + pmSq(z1) - pmSq(x2) - pmSq(y2) - pmSq(z2);
    double a3 = 2 * (x3 - x1);
    double b3 = 2 * (y3 - y1);
    double c3 = 2 * (z3 - z1);
    double d3 = pmSq(x1) + pmSq(y1) + pmSq(z1) - pmSq(x3) - pmSq(y3) - pmSq(z3);
    center_cir->x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    center_cir->y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    center_cir->z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    //r=pmSqrt(pmSq(x1-center_cir->x)+pmSq(y1-center_cir->y)+pmSq(z1-center_cir->z));
    return 0;
}
int RobotProxy::calcTCPErr_dist(ZucPose const* const tool_to_user1,
                                ZucPose const* const tool_to_user2,
                                ZucPose const* const tool_to_user3,
                                ZucPose const* const tool_to_user4,
                                double* const dist_mean,
                                int* const num)  //tcp误差计算
{
    if (!tool_to_user1 || !tool_to_user2 || !tool_to_user3 || !tool_to_user4)
        return -1;
    ZucPose tool_mean;

    tool_mean.tran.x = (tool_to_user1->tran.x + tool_to_user2->tran.x + tool_to_user3->tran.x + tool_to_user4->tran.x) / 4;
    tool_mean.tran.y = (tool_to_user1->tran.y + tool_to_user2->tran.y + tool_to_user3->tran.y + tool_to_user4->tran.y) / 4;
    tool_mean.tran.z = (tool_to_user1->tran.z + tool_to_user2->tran.z + tool_to_user3->tran.z + tool_to_user4->tran.z) / 4;

    dist_mean[0] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user1->tran.x) + pmSq(tool_mean.tran.y - tool_to_user1->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user1->tran.z));
    dist_mean[1] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user2->tran.x) + pmSq(tool_mean.tran.y - tool_to_user2->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user2->tran.z));
    dist_mean[2] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user3->tran.x) + pmSq(tool_mean.tran.y - tool_to_user3->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user3->tran.z));
    dist_mean[3] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user4->tran.x) + pmSq(tool_mean.tran.y - tool_to_user4->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user4->tran.z));
    *num = 4;

    return 0;
}

// tcp 误差计算
int RobotProxy::calcTCPErr_lea_squ(const MatrixXd& A, const VectorXd& B, const VectorXd& retTcp, double* const dist_mean, int* const num)
{
    // if(!A||!B||!retTcp)
    //     return -1;
    PmCartesian error_lea_squ[6] = {0.0};
    for (int i = 0; i < 6; i++)
    {
        error_lea_squ[i].x = (A(i * 3, 0) * retTcp(0) + A(i * 3, 1) * retTcp(1) + A(i * 3, 2) * retTcp(2)) - B(i * 3);
        error_lea_squ[i].y = (A(i * 3 + 1, 0) * retTcp(0) + A(i * 3 + 1, 1) * retTcp(1) + A(i * 3 + 1, 2) * retTcp(2)) - B(i * 3 + 1);
        error_lea_squ[i].z = (A(i * 3 + 2, 0) * retTcp(0) + A(i * 3 + 2, 1) * retTcp(1) + A(i * 3 + 2, 2) * retTcp(2)) - B(i * 3 + 2);
        dist_mean[i] = pmSqrt(pmSq(error_lea_squ[i].x) + pmSq(error_lea_squ[i].y) + pmSq(error_lea_squ[i].z));
    }
    *num = 6;
    return 0;
}

int RobotProxy::zucTrajCalcTcp(const unsigned char id,
                               const unsigned char saveType,
                               const unsigned char statusFlag,
                               const double* jntPos1,
                               const double* jntPos2,
                               const double* jntPos3,
                               const double* jntPos4)
{
    double permissible_min_error = 100;
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPose[4] = {0.0};
    ZucPose flangeTool;
    ZERO_ZUC_POSE(flangeTool);
    if (fabs(jntPos1[0]) + fabs(jntPos1[1]) + fabs(jntPos1[2]) + fabs(jntPos1[3]) + fabs(jntPos1[4]) + fabs(jntPos1[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine()->fkine(RobJointVal(jntPos1), cartPose[0]);
    if (fabs(jntPos2[0]) + fabs(jntPos2[1]) + fabs(jntPos2[2]) + fabs(jntPos2[3]) + fabs(jntPos2[4]) + fabs(jntPos2[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine()->fkine(RobJointVal(jntPos2), cartPose[1]);
    if (fabs(jntPos3[0]) + fabs(jntPos3[1]) + fabs(jntPos3[2]) + fabs(jntPos3[3]) + fabs(jntPos3[4]) + fabs(jntPos3[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine()->fkine(RobJointVal(jntPos3), cartPose[2]);
    if (fabs(jntPos4[0]) + fabs(jntPos4[1]) + fabs(jntPos4[2]) + fabs(jntPos4[3]) + fabs(jntPos4[4]) + fabs(jntPos4[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine()->fkine(RobJointVal(jntPos4), cartPose[3]);

    stat_robotproxy.usrOpeErrCode = 0;
    ZERO_ZUC_POSE(stat_robotproxy.utilCartPos);
    stat_robotproxy.tcpCalError[0] = 0.0;
    stat_robotproxy.tcpCalError[1] = 0.0;
    stat_robotproxy.tcpCalError[2] = 0.0;
    if (saveType == 1)
    {
        //判断四点位姿是否重合
        double dist_p[6] = {0.0};
        int num_dist = 0;
        for (int i = 0; i < 4; i++)
        {
            for (int j = i + 1; j < 4; j++)
            {
                dist_p[num_dist] = pmSqrt(pmSq(cartPose[i].tran.x - cartPose[j].tran.x) + pmSq(cartPose[i].tran.y - cartPose[j].tran.y) +
                                          pmSq(cartPose[i].tran.z - cartPose[j].tran.z));

                if (dist_p[num_dist] < TP_POS_EPSILON)
                {
                    zucOperatorError(REPEATED_TEACH_POINTS, "teach points are repeated");
                    return -1;
                }
                num_dist++;
            }
        }
        PmRpy temprpy;
        PmRotationMatrix rotMat, rotMatList[4];
        VectorXd B(6 * 3), retTcp(3);
        MatrixXd A(6 * 3, 3);
        VectorXd deltaP(6 * 3);
        int mtrixIndex = 0;
        for (int i = 0; i < 4; i++)
        {
            temprpy.y = cartPose[i].a * PM_PI / 180.0;
            temprpy.p = cartPose[i].b * PM_PI / 180.0;
            temprpy.r = cartPose[i].c * PM_PI / 180.0;
            pmRpyMatConvert(&temprpy, &rotMat);
            for (int j = i - 1; j >= 0; j--)
            {
                A(3 * mtrixIndex, 0) = rotMat.x.x - rotMatList[j].x.x;
                A(3 * mtrixIndex, 1) = rotMat.y.x - rotMatList[j].y.x;
                A(3 * mtrixIndex, 2) = rotMat.z.x - rotMatList[j].z.x;
                A(3 * mtrixIndex + 1, 0) = rotMat.x.y - rotMatList[j].x.y;
                A(3 * mtrixIndex + 1, 1) = rotMat.y.y - rotMatList[j].y.y;
                A(3 * mtrixIndex + 1, 2) = rotMat.z.y - rotMatList[j].z.y;
                A(3 * mtrixIndex + 2, 0) = rotMat.x.z - rotMatList[j].x.z;
                A(3 * mtrixIndex + 2, 1) = rotMat.y.z - rotMatList[j].y.z;
                A(3 * mtrixIndex + 2, 2) = rotMat.z.z - rotMatList[j].z.z;
                B(3 * mtrixIndex) = cartPose[j].tran.x - cartPose[i].tran.x;
                B(3 * mtrixIndex + 1) = cartPose[j].tran.y - cartPose[i].tran.y;
                B(3 * mtrixIndex + 2) = cartPose[j].tran.z - cartPose[i].tran.z;
                mtrixIndex++;
            }
            rotMatList[i] = rotMat;
        }

        retTcp = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(B);
        stat_robotproxy.utilCartPos.tran.x = retTcp(0);  //计算得到的工具坐标系
        stat_robotproxy.utilCartPos.tran.y = retTcp(1);
        stat_robotproxy.utilCartPos.tran.z = retTcp(2);
        if (checkCalcTcpResult(stat_robotproxy.utilCartPos, cartPose, 4))
        {
            zucOperatorError(TCP_CALIBRATION_POSITION_OVER_LIMIT, "tcp calibration position is over workspace limit");
            ZERO_ZUC_POSE(stat_robotproxy.utilCartPos);
            return -1;
        }

        axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(id);
        // tool_offset_refv.pose().tran = stat_robotproxy.utilCartPos.tran;
        tool_offset_refv.set_pose(stat_robotproxy.utilCartPos);

        PmRpy base_offset;  //ybh need fix

        ZucPose tool_to_user1, tool_to_user2, tool_to_user3, tool_to_user4;
        axisgroup::AxisGroupMgr::instance()
            .get_axisgroup(stat.rob_id)
            ->kine()
            ->fkine(RobJointVal(jntPos1), tool_to_user1, &stat_robotproxy.utilCartPos, &flangeTool, &base_offset);
        axisgroup::AxisGroupMgr::instance()
            .get_axisgroup(stat.rob_id)
            ->kine()
            ->fkine(RobJointVal(jntPos2), tool_to_user2, &stat_robotproxy.utilCartPos, &flangeTool, &base_offset);
        axisgroup::AxisGroupMgr::instance()
            .get_axisgroup(stat.rob_id)
            ->kine()
            ->fkine(RobJointVal(jntPos3), tool_to_user3, &stat_robotproxy.utilCartPos, &flangeTool, &base_offset);
        axisgroup::AxisGroupMgr::instance()
            .get_axisgroup(stat.rob_id)
            ->kine()
            ->fkine(RobJointVal(jntPos4), tool_to_user4, &stat_robotproxy.utilCartPos, &flangeTool, &base_offset);

        double dist_mean[6] = {0.0};
        int num = 0;
        switch (TCP_ERROR_TYPE)
        {
        case 0: {
            calcTCPErr_dist(&tool_to_user1, &tool_to_user2, &tool_to_user3, &tool_to_user4, dist_mean, &num);
            break;
        }
        case 1: {
            calcTCPErr_lea_squ(A, B, retTcp, dist_mean, &num);
            break;
        }
        case 2: {
            calcTCPErr_center_cir(&tool_to_user1, &tool_to_user2, &tool_to_user3, &tool_to_user4, dist_mean, &num);
            break;
        }
        default: {
            MOTAL_INFO("tcp error type choice failed\n");
            return -1;
        }
        }

        double max_error = dist_mean[0], min_error = dist_mean[0], mean_error = dist_mean[0];

        for (int i = 1; i < num; i++)
        {
            if (max_error < dist_mean[i])
            {
                max_error = dist_mean[i];
            }
            if (min_error > dist_mean[i])
            {
                min_error = dist_mean[i];
            }
            mean_error += dist_mean[i];
        }
        mean_error = mean_error / num;
        stat_robotproxy.tcpCalError[0] = max_error;
        stat_robotproxy.tcpCalError[1] = min_error;
        stat_robotproxy.tcpCalError[2] = mean_error;
        MOTAL_INFO("max error is %f, min error is %f, mean error is %f\n", max_error, min_error, mean_error);
        if (permissible_min_error < min_error)
        {
            zucOperatorError(TCP_CALIBRATION_ERROR_TOO_BIG, "tcp calibration error is too big");
            MOTAL_INFO("min error is %f less then permissible min error is %f\n", min_error, permissible_min_error);
            stat_robotproxy.tcpCalError[0] = 0.0;
            stat_robotproxy.tcpCalError[1] = 0.0;
            stat_robotproxy.tcpCalError[2] = 0.0;
            return -1;
        }
    }
    iniSaveTCPCalibration(0, id, saveType, statusFlag, cartPose, jntPos1, jntPos2, jntPos3, jntPos4, 0, 0, stat_robotproxy.tcpCalError);
    return 0;
}

//iniflag 六点法计算TCP
int RobotProxy::zucTrajCalcTcp_six(const unsigned char id,
                                   const unsigned char saveType,
                                   const unsigned char statusFlag,
                                   const double* jntPos1,
                                   const double* jntPos2,
                                   const double* jntPos3,
                                   const double* jntPos4,
                                   const double* jntPos5,
                                   const double* jntPos6)
{
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPose[6] = {0.0};
    ZucPose flangeTool;
    ZERO_ZUC_POSE(flangeTool);
    stat_robotproxy.usrOpeErrCode = 0;

    // calculate TCP position using 4-point method
    if (zucTrajCalcTcp(id, saveType, statusFlag, jntPos1, jntPos2, jntPos3, jntPos4) != 0)
    {
        return -1;
    }

    /* calculate TCP gesture */
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();
    if (fabs(jntPos4[0]) + fabs(jntPos4[1]) + fabs(jntPos4[2]) + fabs(jntPos4[3]) + fabs(jntPos4[4]) + fabs(jntPos4[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos4), cartPose[3]);
    if (fabs(jntPos5[0]) + fabs(jntPos5[1]) + fabs(jntPos5[2]) + fabs(jntPos5[3]) + fabs(jntPos5[4]) + fabs(jntPos5[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos5), cartPose[4]);
    if (fabs(jntPos6[0]) + fabs(jntPos6[1]) + fabs(jntPos6[2]) + fabs(jntPos6[3]) + fabs(jntPos6[4]) + fabs(jntPos6[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos6), cartPose[5]);

    if (saveType)
    {
        //判断四五六点是否共线(共点可以接受)
        PmCartesian vec34, vec45, vec35;
        vec34.x = cartPose[4].tran.x - cartPose[3].tran.x;
        vec34.y = cartPose[4].tran.y - cartPose[3].tran.y;
        vec34.z = cartPose[4].tran.z - cartPose[3].tran.z;

        vec45.x = cartPose[5].tran.x - cartPose[4].tran.x;
        vec45.y = cartPose[5].tran.y - cartPose[4].tran.y;
        vec45.z = cartPose[5].tran.z - cartPose[4].tran.z;

        vec35.x = cartPose[5].tran.x - cartPose[3].tran.x;
        vec35.y = cartPose[5].tran.y - cartPose[3].tran.y;
        vec35.z = cartPose[5].tran.z - cartPose[3].tran.z;

        double vec34_mags = vec34.x * vec34.x + vec34.y * vec34.y + vec34.z * vec34.z;
        double vec45_mags = vec45.x * vec45.x + vec45.y * vec45.y + vec45.z * vec45.z;
        double vec35_mags = vec35.x * vec35.x + vec35.y * vec35.y + vec35.z * vec35.z;
        int three_p_repeat = (vec34_mags > 1e-8 && vec45_mags > 1e-8 && vec35_mags > 1e-8) ? 0 : 1;
        if (three_p_repeat)
        {
            zucOperatorError(REPEATED_TEACH_POINTS, "teach points are repeated");
            return -1;
        }
        pmCartUnit(&vec34, &vec34);
        pmCartUnit(&vec45, &vec45);
        PmCartesian corss_ret;
        pmCartCartCross(&vec34, &vec45, &corss_ret);
        double corss_ret_mag = pmSqrt(corss_ret.x * corss_ret.x + corss_ret.y * corss_ret.y + corss_ret.z * corss_ret.z);
        if (corss_ret_mag < 1e-4 && !three_p_repeat)
        {
            zucOperatorError(TEACH_POINTS_COLLINEAR, "teach points are collinear");
            return -1;
        }

        PmRpy temprpy;
        PmRotationMatrix rotMat, utilrotTcp, rotbf;
        Matrix3f rotfb(3, 3), rotTcptoB(3, 3), retrotTcp(3, 3);
        temprpy.y = cartPose[3].a * PM_PI / 180.0;
        temprpy.p = cartPose[3].b * PM_PI / 180.0;
        temprpy.r = cartPose[3].c * PM_PI / 180.0;
        pmRpyMatConvert(&temprpy, &rotMat);
        rotfb << rotMat.x.x, rotMat.y.x, rotMat.z.x, rotMat.x.y, rotMat.y.y, rotMat.z.y, rotMat.x.z, rotMat.y.z, rotMat.z.z;
        rotbf.x.x = rotMat.x.x;
        rotbf.x.y = rotMat.y.x;
        rotbf.x.z = rotMat.z.x;
        rotbf.y.x = rotMat.x.y;
        rotbf.y.y = rotMat.y.y;
        rotbf.y.z = rotMat.z.y;
        rotbf.z.x = rotMat.x.z;
        rotbf.z.y = rotMat.y.z;
        rotbf.z.z = rotMat.z.z;
        pmMatRpyConvert(&rotbf, &temprpy);

        Vector3f rotTcptoB_z(cartPose[4].tran.x - cartPose[3].tran.x, cartPose[4].tran.y - cartPose[3].tran.y, cartPose[4].tran.z - cartPose[3].tran.z);
        Vector3f rotTcptoB_x(cartPose[5].tran.x - cartPose[4].tran.x, cartPose[5].tran.y - cartPose[4].tran.y, cartPose[5].tran.z - cartPose[4].tran.z);
        Vector3f rotTcptoB_y;
        rotTcptoB_y = rotTcptoB_z.cross(rotTcptoB_x);
        rotTcptoB_x = rotTcptoB_y.cross(rotTcptoB_z);
        rotTcptoB_x.normalize();
        rotTcptoB_y.normalize();
        rotTcptoB_z.normalize();
        rotTcptoB << rotTcptoB_x(0), rotTcptoB_y(0), rotTcptoB_z(0), rotTcptoB_x(1), rotTcptoB_y(1), rotTcptoB_z(1), rotTcptoB_x(2), rotTcptoB_y(2),
            rotTcptoB_z(2);
        MOTAL_INFO("the y,p,r of world to flange are: %f, %f, %f\n", temprpy.y * 180 / PM_PI, temprpy.p * 180 / PM_PI, temprpy.r * 180 / PM_PI);

        retrotTcp = rotfb.inverse() * rotTcptoB;
        utilrotTcp.x.x = retrotTcp(0, 0);
        utilrotTcp.x.y = retrotTcp(1, 0);
        utilrotTcp.x.z = retrotTcp(2, 0);
        utilrotTcp.y.x = retrotTcp(0, 1);
        utilrotTcp.y.y = retrotTcp(1, 1);
        utilrotTcp.y.z = retrotTcp(2, 1);
        utilrotTcp.z.x = retrotTcp(0, 2);
        utilrotTcp.z.y = retrotTcp(1, 2);
        utilrotTcp.z.z = retrotTcp(2, 2);
        pmMatRpyConvert(&utilrotTcp, &temprpy);

        stat_robotproxy.utilCartPos.a = temprpy.y * 180 / PM_PI;
        stat_robotproxy.utilCartPos.b = temprpy.p * 180 / PM_PI;
        stat_robotproxy.utilCartPos.c = temprpy.r * 180 / PM_PI;
    }
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(id);
    // tool_offset_refv.pose.a = stat_robotproxy.utilCartPos.a;
    // tool_offset_refv.pose.b = stat_robotproxy.utilCartPos.b;
    // tool_offset_refv.pose.c = stat_robotproxy.utilCartPos.c;
    tool_offset_refv.set_pose(stat_robotproxy.utilCartPos);
    iniSaveTCPCalibration(1, id, saveType, statusFlag, cartPose, jntPos1, jntPos2, jntPos3, jntPos4, jntPos5, jntPos6, 0);
    return 0;
}

int RobotProxy::calcTCPErr_center_cir(ZucPose const* const tool_to_user1,
                                      ZucPose const* const tool_to_user2,
                                      ZucPose const* const tool_to_user3,
                                      ZucPose const* const tool_to_user4,
                                      double* const dist_mean,
                                      int* const num)
{
    if (!tool_to_user1 || !tool_to_user2 || !tool_to_user3 || !tool_to_user4)
        return -1;
    PmCartesian cneter_cir1, cneter_cir2, cneter_cir3, cneter_cir4, center_mean;
    double r1, r2, r3, r4;
    calculate_center_circle(tool_to_user1, tool_to_user2, tool_to_user3, &cneter_cir1);
    calculate_center_circle(tool_to_user1, tool_to_user2, tool_to_user4, &cneter_cir2);
    calculate_center_circle(tool_to_user1, tool_to_user3, tool_to_user4, &cneter_cir3);
    calculate_center_circle(tool_to_user2, tool_to_user3, tool_to_user4, &cneter_cir4);
    center_mean.x = (cneter_cir1.x + cneter_cir2.x + cneter_cir3.x + cneter_cir4.x) / 4;
    center_mean.y = (cneter_cir1.y + cneter_cir2.y + cneter_cir3.y + cneter_cir4.y) / 4;
    center_mean.z = (cneter_cir1.z + cneter_cir2.z + cneter_cir3.z + cneter_cir4.z) / 4;
    dist_mean[0] =
        pmSqrt(pmSq(center_mean.x - tool_to_user1->tran.x) + pmSq(center_mean.y - tool_to_user1->tran.y) + pmSq(center_mean.z - tool_to_user1->tran.z));
    dist_mean[1] =
        pmSqrt(pmSq(center_mean.x - tool_to_user2->tran.x) + pmSq(center_mean.y - tool_to_user2->tran.y) + pmSq(center_mean.z - tool_to_user2->tran.z));
    dist_mean[2] =
        pmSqrt(pmSq(center_mean.x - tool_to_user3->tran.x) + pmSq(center_mean.y - tool_to_user3->tran.y) + pmSq(center_mean.z - tool_to_user3->tran.z));
    dist_mean[3] =
        pmSqrt(pmSq(center_mean.x - tool_to_user4->tran.x) + pmSq(center_mean.y - tool_to_user4->tran.y) + pmSq(center_mean.z - tool_to_user4->tran.z));
    *num = 4;
    return 0;
}

int RobotProxy::zucTrajCalcUser(const unsigned char id,
                                const unsigned char saveType,
                                const unsigned char statusFlag,
                                const double* jntPos1,
                                const double* jntPos2,
                                const double* jntPos3)  //iniflag 计算用户坐标系
{
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPos1 = {0.0}, cartPos2 = {0.0}, cartPos3 = {0.0};
    ZucPose flangeTool;
    ZucPose flangeUser;
    PmCartesian x0, y0, z0;
    double cos_theta /* , magB */;
    PmRotationMatrix rot;
    PmRpy rpy;
    ZERO_ZUC_POSE(flangeUser);
    //ybh need fix
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);

    flangeTool = tool_offset_refv.pose();
    PmRpy baseoffset;
    baseoffset.y = base_offset_refv.pose().a;
    baseoffset.p = base_offset_refv.pose().b;
    baseoffset.r = base_offset_refv.pose().c;

    rcs_print("[ zucTrajCalcUser ]: flangeTool =     %f, %f, %f\n", flangeTool.tran.x, flangeTool.tran.y, flangeTool.tran.z);
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();
    if (fabs(jntPos1[0]) + fabs(jntPos1[1]) + fabs(jntPos1[2]) + fabs(jntPos1[3]) + fabs(jntPos1[4]) + fabs(jntPos1[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos1), cartPos1, &flangeTool, &flangeUser, &baseoffset);
    rcs_print("[ zucTrajCalcUser ]: CartPos1 is: %f, %f, %f\n", cartPos1.tran.x, cartPos1.tran.y, cartPos1.tran.z);
    if (fabs(jntPos2[0]) + fabs(jntPos2[1]) + fabs(jntPos2[2]) + fabs(jntPos2[3]) + fabs(jntPos2[4]) + fabs(jntPos2[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos2), cartPos2, &flangeTool, &flangeUser, &baseoffset);
    rcs_print("[ zucTrajCalcUser ]: CartPos2 is: %f, %f, %f\n", cartPos2.tran.x, cartPos2.tran.y, cartPos2.tran.z);
    if (fabs(jntPos3[0]) + fabs(jntPos3[1]) + fabs(jntPos3[2]) + fabs(jntPos3[3]) + fabs(jntPos3[4]) + fabs(jntPos3[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos3), cartPos3, &flangeTool, &flangeUser, &baseoffset);
    rcs_print("[ zucTrajCalcUser ]: CartPos3 is: %f, %f, %f\n", cartPos3.tran.x, cartPos3.tran.y, cartPos3.tran.z);
    stat_robotproxy.usrOpeErrCode = 0;
    ZERO_ZUC_POSE(stat_robotproxy.utilCartPos);
    if (saveType || id == 0)  // 执行MoveZ、MoveZS指令时 APP生成的jks会根据指令参数临时修改世界坐标系。
    {
        int ret_zero_x0 = 0, ret_zero_y0 = 0;
        stat_robotproxy.utilCartPos.tran = cartPos1.tran;
        pmCartCartSub(&cartPos2.tran, &cartPos1.tran, &x0);
        ret_zero_x0 = pmCheckCartZero(&x0);
        pmCartUnitEq(&x0);
        pmCartCartSub(&cartPos3.tran, &cartPos2.tran, &y0);
        ret_zero_y0 = pmCheckCartZero(&y0);
        pmCartUnitEq(&y0);
        pmCartCartDot(&x0, &y0, &cos_theta);
        //余弦值判断向量v12和v23是否共线，坐标值求和判断是否为零向量
        if ((1 - fabs(cos_theta)) < 0.001 || ret_zero_x0 || ret_zero_y0)
        {
            zucOperatorError(USER_CALIBRATION_FAILED,
                             "user frame calibration failed! cos_theta: %f, Vector12: <%f %f %f>, Vector23: <%f %f %f> \n",
                             cos_theta,
                             x0.x,
                             x0.y,
                             x0.z,
                             y0.x,
                             y0.y,
                             y0.z);
            ZERO_ZUC_POSE(stat_robotproxy.utilCartPos);
            return -1;
        }

        pmCartCartCross(&x0, &y0, &z0);
        pmCartUnitEq(&z0);
        pmCartCartCross(&z0, &x0, &y0);
        rot.x = x0;
        rot.y = y0;
        rot.z = z0;
        pmMatRpyConvert(&rot, &rpy);
        stat_robotproxy.utilCartPos.a = rpy.y * 180 / PM_PI;
        stat_robotproxy.utilCartPos.b = rpy.p * 180 / PM_PI;
        stat_robotproxy.utilCartPos.c = rpy.r * 180 / PM_PI;
    }
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(id);
    user_offset_refv.set_pose(stat_robotproxy.utilCartPos);
    iniSaveUserCalibration(id, statusFlag, cartPos1, cartPos2, cartPos3, jntPos1, jntPos2, jntPos3);
    return 0;
}

int RobotProxy::zucTrajSetTermCond(int cond, double tolerance)  //motion 设置转接相关
{
    TOPP.last_tc_blend_r = TOPP.cur_tc_blend_r;
    TOPP.cur_tc_blend_r = tolerance;  // update cur_tc_blend_r for toppra

    auto cmd = std::make_shared<CmdSetTermCond>(stat.rob_id, cond, tolerance);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::cal_using_abc(ZucPose start_cartpos,
                              ZucPose end,
                              double vel,
                              double acc,
                              double ori_vel,
                              double ori_acc,
                              int* using_abc,
                              double* tmag_abc,
                              ZucPose trajlinearmov_pos)  //motionflag task计算运动主轴
{
    int ret = 0;
    double delta_x = fabs(start_cartpos.tran.x - end.tran.x);
    double delta_y = fabs(start_cartpos.tran.y - end.tran.y);
    double delta_z = fabs(start_cartpos.tran.z - end.tran.z);
    double delta_rx = fabs(start_cartpos.a - end.a);
    double delta_ry = fabs(start_cartpos.b - end.b);
    double delta_rz = fabs(start_cartpos.c - end.c);
    int repeat_position = 0;
    int repeat_ori = 0;
    if ((delta_x < SMALL) && (delta_y < SMALL) && (delta_z < SMALL))
    {
        repeat_position = 1;
    }
    if ((delta_rx < SMALL) && (delta_ry < SMALL) && (delta_rz < SMALL))
    {
        repeat_ori = 1;
    }
    double delta_cart_len = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
    double delta_ori_len = 0.0;
    if (!repeat_ori)
    {
        //get angle
        PmRpy rpyTarget, rpyCurr;
        PmQuaternion quatTarget, quatCurr, quatDelt;
        rpyCurr.y = start_cartpos.a * PM_PI / 180.0;
        rpyCurr.p = start_cartpos.b * PM_PI / 180.0;
        rpyCurr.r = start_cartpos.c * PM_PI / 180.0;
        rpyTarget.y = trajlinearmov_pos.a * PM_PI / 180.0;
        rpyTarget.p = trajlinearmov_pos.b * PM_PI / 180.0;
        rpyTarget.r = trajlinearmov_pos.c * PM_PI / 180.0;

        pmRpyQuatConvert(&rpyTarget, &quatTarget);
        pmRpyQuatConvert(&rpyCurr, &quatCurr);
        pmQuatInv(&quatCurr, &quatCurr);
        pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
        if (quatDelt.s > 1.0)
            quatDelt.s = 1.0;
        else if (quatDelt.s < -1.0)
            quatDelt.s = -1.0;
        *tmag_abc = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
        delta_ori_len = *tmag_abc;
    }
    if (repeat_position && repeat_ori)
    {
        ret = 1;  //repeat point
    }
    else if (repeat_position)
    {
        *using_abc = 1;
    }
    else if (repeat_ori)
    {
        *using_abc = 0;
    }
    else
    {
        double triangle_vel = sqrt(acc * delta_cart_len);
        double t_for_pos = 0.0;
        if (triangle_vel < vel)
        {
            t_for_pos = 2.0 * delta_cart_len / triangle_vel;
        }
        else
        {
            double t_acc = vel / acc;
            double t_dec = t_acc;
            double t_const = (delta_cart_len - (t_acc * vel)) / vel;
            t_for_pos = t_acc + t_const + t_dec;
        }
        MOTAL_INFO(" acc = %f, vel = %f, triangle_vel = %f, t_for_pos = %f \n", acc, vel, triangle_vel, t_for_pos);
        MOTAL_INFO(" -------------------------- tmag_abc = %f, delta_cart_len = %f -------------------------- \n", *tmag_abc, delta_cart_len);
        double triangle_vel_ori = sqrt(delta_ori_len * ori_acc);
        double t_for_ori = 0.0;
        if (triangle_vel_ori < ori_vel)
        {
            t_for_ori = 2.0 * delta_ori_len / triangle_vel_ori;
        }
        else
        {
            double t_acc = ori_vel / ori_acc;
            double t_dec = t_acc;
            double t_const = (delta_ori_len - (t_acc * ori_vel)) / ori_vel;
            t_for_ori = t_acc + t_const + t_dec;
        }
        MOTAL_INFO(" ori_acc = %f, ori_vel = %f, triangle_vel_ori = %f, t_for_ori = %f \n", ori_acc, ori_vel, triangle_vel_ori, t_for_ori);

        if (t_for_ori >= t_for_pos)
        {
            *using_abc = 1;
        }
        else
        {
            *using_abc = 0;
        }
    }
    return ret;
}

int RobotProxy::zucTrajLinearMove(ZucPose end,
                                  int type,
                                  double vel,
                                  double ini_maxvel,
                                  double acc,
                                  double jerk,
                                  double ori_vel,
                                  double ori_acc,
                                  int indexrotary,
                                  int using_abc,
                                  int di_type,
                                  int di_index,
                                  int di_state,
                                  int planner_type)  //motionflag 解析添加直线运动段
{
    if (stat.protective_stop == 1 && stat_robotproxy.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do linear move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;

    zuc_traj_linearmov_t trajlinearmov = {};
    int id = stat.config.TrajConfig.MotionId;

    // MOTAL_INFO("ZUCMOT_SET_LINE ---> end = %f %f %f %f %f %f\n", end.tran.x, end.tran.y, end.tran.z, end.a, end.b, end.c);
    trajlinearmov.pos = end;
    trajlinearmov.motion_type = type;
    trajlinearmov.vel = vel;
    trajlinearmov.ini_maxvel = ini_maxvel;
    trajlinearmov.acc = acc;
    trajlinearmov.turn = indexrotary;
    trajlinearmov.jerk = jerk;
    trajlinearmov.ori_vel = ori_vel;
    trajlinearmov.ori_acc = ori_acc;
    trajlinearmov.using_abc_vel = using_abc;
    trajlinearmov.di_type = di_type;
    trajlinearmov.di_state = di_state;
    trajlinearmov.di_index = di_index;
    trajlinearmov.planner_type = planner_type;

    if ((!stat.userset_use_toppra || TOPP.kinematic_error) && trajlinearmov.planner_type == TOPP_PLANNER)
    {
        MOTAL_INFO("******************************** [unable use toppra] ---> set planner_type = T_planner *******************************\n");
        trajlinearmov.planner_type = T_PLANNER;  //配置文件中没有开启时间最优时默认使用T规划
    }
    if (trajlinearmov.planner_type == S_PLANNER && trajlinearmov.jerk < 1e-6)
    {
        trajlinearmov.jerk = 0.0;
        trajlinearmov.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }

    tc_motion_type_t motion_type = TC_LINEAR;

    double safety_func_vel = get_vel_by_stopdisttime(jerk, motion_type, trajlinearmov.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajlinearmov.vel = safety_func_vel;
        trajlinearmov.ini_maxvel = safety_func_vel;
    }
    clock_t start_t, finish_t;
    double duration;
    if (TOPP.first_command)
    {
        get_last_pos();
        TOPP.first_command = 0;
    }
    kine::KineItf::kine_Conf toppra_conf;
    ZucPose start_cartpos;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();
    //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
    kine->fkine(RobJointVal(stat_robotproxy.cur_tc_goal_jointpos), start_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset_refv.pose());
    if (trajlinearmov.planner_type == TOPP_PLANNER)
    {
        //计算规划主轴
        double tmag_abc = 0.0;
        if (trajlinearmov.vel > TOPP.tcp_vel_limit_toppra)
        {
            MOTAL_INFO(" ----------------------------------------------- trajlinearmov.vel = %f, tcp_vel_limit_toppra = %f\n",
                       trajlinearmov.vel,
                       TOPP.tcp_vel_limit_toppra);
            trajlinearmov.vel = TOPP.tcp_vel_limit_toppra;
            trajlinearmov.ini_maxvel = TOPP.tcp_vel_limit_toppra;
        }
        int cal_ret = cal_using_abc(start_cartpos, end, trajlinearmov.vel, acc, ori_vel, ori_acc, &trajlinearmov.using_abc_vel, &tmag_abc, trajlinearmov.pos);
        if (cal_ret != 0)
        {
            stat.motion.already_inpos = 1;
            TOPP.cur_tc_blend_r = 0.0;
            TOPP.last_tc_blend_r = 0.0;
            trajlinearmov.planner_type = T_PLANNER;
        }
        else
        {
            MOTAL_INFO("trajlinearmov.using_abc_vel = %d, tmag_abc = %f\n", trajlinearmov.using_abc_vel, tmag_abc);
            start_t = clock();  //计时
            MOTAL_INFO("[1]last_tc_blend_r = %f\n", TOPP.last_tc_blend_r);
            // if (movl_get_toppra_params(start_cartpos, toppra_conf, tmag_abc) != 0)
            // {
            //     trajlinearmov.planner_type = T_PLANNER;
            //     MOTAL_INFO("[movl_get_toppra_params != 0]   --->   [planner_type = T_PLANNER]\n");
            // }
            finish_t = clock();
            duration = (double)(finish_t - start_t) / CLOCKS_PER_SEC;
            MOTAL_INFO(" ======================================================================= movl_get_toppra_params = %fms "
                       "======================================================================= \n",
                       duration * 1000);
            if (!stat.motion.already_inpos && trajlinearmov.planner_type == TOPP_PLANNER)
            {
                update_related_params();
                // MOTAL_INFO(" [TOPPRA]   ************************************ cur_tc_goal_jointpos = %f %f %f %f %f %f\n",
                //             cur_tc_goal_jointpos[0],
                //             cur_tc_goal_jointpos[1],
                //             cur_tc_goal_jointpos[2],
                //             cur_tc_goal_jointpos[3],
                //             cur_tc_goal_jointpos[4],
                //             cur_tc_goal_jointpos[5]);
            }
            else
            {
                RobJointVal jpos(stat_robotproxy.cur_tc_goal_jointpos);
                // kine::KineItf::kine_Conf conf;
                auto inv_ret = kine->ikine(end, jpos, toppra_conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset_refv.pose());
                jpos.array_value(stat_robotproxy.cur_tc_goal_jointpos);
                // MOTAL_INFO(" ************************************ cur_tc_goal_jointpos = %f %f %f %f %f %f\n",
                //             cur_tc_goal_jointpos[0],
                //             cur_tc_goal_jointpos[1],
                //             cur_tc_goal_jointpos[2],
                //             cur_tc_goal_jointpos[3],
                //             cur_tc_goal_jointpos[4],
                //             cur_tc_goal_jointpos[5]);
                if (inv_ret != 0)
                {
                    TOPP.kinematic_error = 1;
                }
                else
                {
                    update_related_params2();
                }
            }
        }
    }
    else
    {
        RobJointVal jpos(stat_robotproxy.cur_tc_goal_jointpos);
        kine::KineItf::kine_Conf conf;
        auto inv_ret = kine->ikine(end, jpos, conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        jpos.array_value(stat_robotproxy.cur_tc_goal_jointpos);
        if (inv_ret != 0)
        {
            TOPP.kinematic_error = 1;
        }
        else
        {
            update_related_params2();
        }
    }
    auto cmd = std::make_shared<CmdSetLine>(stat.rob_id, id, motion_type, trajlinearmov);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time)  //motionflag 解析添加movs
{
    int i = 0;
    zuc_traj_movs_t trajmovs = {};
    int id = stat.config.TrajConfig.MotionId;
    // for (i = 0; i < stat.robot_joint_num; i++)
    for (i = 0; i < 6; i++)
    {
        trajmovs.a0[i] = a0[i];
        trajmovs.a1[i] = a1[i];
        trajmovs.a2[i] = a2[i];
        trajmovs.a3[i] = a3[i];
    }
    trajmovs.time = time;
    TOPP.kinematic_error = 1;

    auto cmd = std::make_shared<CmdSetMovs>(stat.rob_id, id, trajmovs);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucTrajMovc(ZucPose mid,
                            ZucPose end,
                            double vel,
                            double ini_maxvel,
                            double acc,
                            double jerk,
                            int using_abc,
                            double circle_count,
                            int di_type,
                            int di_index,
                            int di_state,
                            int planner_type,
                            int circlemode)  //motionflag 解析添加movc
{
    if (stat.protective_stop == 1 && stat_robotproxy.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do movc move when in protective_stop status\n");
        return -1;
    }
#ifdef ISNAN_TRAP
    if (std::isnan(mid.tran.x) || std::isnan(mid.tran.y) || std::isnan(mid.tran.z) || std::isnan(mid.a) || std::isnan(mid.b) || std::isnan(mid.c) ||
        std::isnan(mid.u) || std::isnan(mid.v) || std::isnan(mid.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    zuc_traj_movc_t trajmovc = {};
    int id = stat.config.TrajConfig.MotionId;

    trajmovc.movcMid = mid;
    trajmovc.movcEnd = end;
    trajmovc.vel = vel;
    trajmovc.ini_maxvel = ini_maxvel;
    trajmovc.acc = acc;
    trajmovc.jerk = jerk;
    trajmovc.using_abc_vel = using_abc;
    trajmovc.circle_count = circle_count;
    trajmovc.di_type = di_type;
    trajmovc.di_state = di_state;
    trajmovc.di_index = di_index;
    trajmovc.circlemode = circlemode;

    trajmovc.planner_type = planner_type;
    if (((!stat.userset_use_toppra || TOPP.kinematic_error) && trajmovc.planner_type == TOPP_PLANNER) || trajmovc.circle_count > 1e-4 || circlemode != 0)
    {
        MOTAL_INFO(
            "******************************** [unable use toppra] ---> planner_type = T_PLANNER, kinematic_error = %d, circle_count = %f, circlemode = %d "
            "*******************************\n",
            TOPP.kinematic_error,
            trajmovc.circle_count,
            circlemode);
        trajmovc.planner_type = T_PLANNER;  //配置文件中没有开启时间最优或整圆运动时默认使用T规划
    }
    if (trajmovc.planner_type == S_PLANNER && trajmovc.jerk < 1e-6)
    {
        trajmovc.jerk = 0.0;
        trajmovc.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }
    stat.motion.motion_type = TC_MOVC;

    double safety_func_vel = get_vel_by_stopdisttime(jerk, stat.motion.motion_type, trajmovc.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajmovc.vel = safety_func_vel;
        trajmovc.ini_maxvel = safety_func_vel;
    }
    clock_t start_t, finish_t;
    double duration;
    if (TOPP.first_command)
    {
        get_last_pos();
        TOPP.first_command = 0;
    }

    if (trajmovc.planner_type == TOPP_PLANNER)
    {
        if (trajmovc.vel > TOPP.tcp_vel_limit_toppra)
        {
            trajmovc.vel = TOPP.tcp_vel_limit_toppra;
            trajmovc.ini_maxvel = TOPP.tcp_vel_limit_toppra;
        }
        start_t = clock();  //计时
        MOTAL_INFO("[movc 1]last_tc_blend_r = %f\n", TOPP.last_tc_blend_r);
        if (movc_get_toppra_params() != 0)
        {
            trajmovc.planner_type = T_PLANNER;
            MOTAL_INFO("[movc_get_toppra_params != 0]   --->  SET planner_type = T_PLANNER\n");
        }
        finish_t = clock();
        duration = (double)(finish_t - start_t) / CLOCKS_PER_SEC;
        MOTAL_INFO(" ======================================================================= movc_get_toppra_params = %fms "
                   "======================================================================= \n",
                   duration * 1000);
        if (!stat.motion.already_inpos)
        {
            update_related_params();
        }

        if (stat.motion.already_inpos)
        {
            TOPP.cur_tc_blend_r = 0.0;
            TOPP.last_tc_blend_r = 0.0;
            MOTAL_INFO(" --------------------------------------------------- movc stat.motion.already_inpos, last_tc_blend_r = %f "
                       "--------------------------------------------------- \n",
                       TOPP.last_tc_blend_r);
            // return 0;
        }
    }
    kine::KineItf::kine_Conf toppra_conf;

    if (trajmovc.planner_type != TOPP_PLANNER)
    {
        ZucPose start_cartpos;
        // kinematicsForward(&(zucmotConfig.tool_offset),
        //                   stat_robotproxy.cur_tc_goal_jointpos,
        //                   &start_cartpos,
        //                   &toppra_fflags,
        //                   &toppra_iflags,
        //                   &(zucmotConfig.base_offset),
        //                   &(zucmotConfig.user_offset));
        // ZucPose start_cartpos;
        auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();
        //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
        kine->fkine(RobJointVal(stat_robotproxy.cur_tc_goal_jointpos), start_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        // int inv_ret = kinematicsInverse(
        //     &zucmotConfig.tool_offset, &end, stat_robotproxy.cur_tc_goal_jointpos, &toppra_iflags, &toppra_fflags, &zucmotConfig.base_offset, &zucmotConfig.user_offset);
        RobJointVal jpos(stat_robotproxy.cur_tc_goal_jointpos);
        // kine::KineItf::kine_Conf conf;
        auto inv_ret = kine->ikine(end, jpos, toppra_conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        jpos.array_value(stat_robotproxy.cur_tc_goal_jointpos);
        if (inv_ret != 0 || trajmovc.circle_count > 1e-4)
        {
            TOPP.kinematic_error = 1;
        }
        else
        {
            update_related_params2();
        }
    }

    auto cmd = std::make_shared<CmdSetMovc>(stat.rob_id, id, stat.motion.motion_type, trajmovc);
    return mot::realtime_cmd_call(cmd);
}

//motionfalg 添加圆运动
int RobotProxy::zucTrajCircularMove(ZucPose end,
                                    PM_CARTESIAN center,
                                    PM_CARTESIAN normal,
                                    int turn,
                                    int type,
                                    double vel,
                                    double ini_maxvel,
                                    double acc,
                                    double jerk)
{
    if (stat.protective_stop == 1 && stat_robotproxy.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do circular move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w) || std::isnan(center.x) || std::isnan(center.y) || std::isnan(center.z) ||
        std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z))
    {
        printf("std::isnan error in zucTrajCircularMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    zuc_traj_circular_mov_t trajcircularmove;
    int id = stat.config.TrajConfig.MotionId;

    trajcircularmove.pos = end;
    trajcircularmove.motion_type = type;
    trajcircularmove.center.x = center.x;
    trajcircularmove.center.y = center.y;
    trajcircularmove.center.z = center.z;
    trajcircularmove.normal.x = normal.x;
    trajcircularmove.normal.y = normal.y;
    trajcircularmove.normal.z = normal.z;
    trajcircularmove.turn = turn;
    trajcircularmove.vel = vel;
    trajcircularmove.ini_maxvel = ini_maxvel;
    trajcircularmove.acc = acc;
    trajcircularmove.jerk = jerk;

    auto cmd = std::make_shared<CmdSetCircle>(stat.rob_id, id, trajcircularmove);
    return mot::realtime_cmd_call(cmd);
}

double RobotProxy::get_vel_by_stopdisttime(double jerk, int motion_type, int planner_type)  //motionflag safeflag 根据停止时间计算速度
{
    double ret_vel = 10000.0;
    double v_time_limit = 10000.0;
    double v_dist_limit = 10000.0;
    double max_acc = 10000.0;
    switch (motion_type)
    {
    case TC_JOINT:
        max_acc = (720.0 < stat.config.JointConfig[0].MaxAccel ? 720.0 : stat.config.JointConfig[0].MaxAccel);  //需要改进，后面加速度值不一定被限制在720以内
        break;
    case TC_LINEAR:
        //如果规划主轴的确定是在此之前确定的，那么这里需要根据规划主轴的选择情况确定最大加速度值
        if (stat_robotproxy.trajlinearmov.using_abc_vel)
        {
            max_acc = (720.0 < stat.config.AxisConfig[3].MaxAccel ? 720.0 : stat.config.AxisConfig[3].MaxAccel);
        }
        else
        {
            max_acc = (6000.0 < stat.config.AxisConfig[0].MaxAccel ? 6000.0 : stat.config.AxisConfig[0].MaxAccel);
        }

        break;
    case TC_MOVC:
        max_acc = (6000.0 < stat.config.AxisConfig[0].MaxAccel ? 6000.0 : stat.config.AxisConfig[0].MaxAccel);
        break;
    default:
        break;
    }
    if (planner_type != S_PLANNER)
    {
        if (planner_type == TOPP_PLANNER)
        {
            double real_lim_time = stat.config.safety.safetyCfg.stoppingTime - stat.trajCycleTime * TOPP.userset_filter_size;
            if (real_lim_time < 0.0)
            {
                real_lim_time = stat.config.safety.safetyCfg.stoppingTime;
                MOTAL_INFO("[get_vel_by_stopdisttime] : real_lim_time < 0.0, stoppingTime = %f, filter_size = %d\n",
                           stat.config.safety.safetyCfg.stoppingTime,
                           TOPP.userset_filter_size);
                TOPP.userset_filter_size = 0;
            }
            v_time_limit = max_acc * real_lim_time;
        }
        else
        {
            //T planner
            v_time_limit = max_acc * stat.config.safety.safetyCfg.stoppingTime;
        }
        if (motion_type == TC_MOVC || motion_type == TC_LINEAR)
        {
            v_dist_limit = sqrt(2 * max_acc * stat.config.safety.safetyCfg.stoppingDist);
        }
    }
    else
    {
        //S planner
        //calculate vel constrainted by stop time
        double t_zero_to_maxacc = max_acc / jerk;
        double t_maxacc_to_zero = t_zero_to_maxacc;
        double t_const_acc = 0.0;
        if ((t_zero_to_maxacc + t_maxacc_to_zero) >= stat.config.safety.safetyCfg.stoppingTime)
        {
            //no const acc : triangle are
            v_time_limit = 0.25 * jerk * stat.config.safety.safetyCfg.stoppingTime * stat.config.safety.safetyCfg.stoppingTime;
        }
        else
        {
            //has const acc: trapezoid area
            t_const_acc = stat.config.safety.safetyCfg.stoppingTime - (t_zero_to_maxacc + t_maxacc_to_zero);
            v_time_limit = 0.5 * max_acc * (stat.config.safety.safetyCfg.stoppingTime + t_const_acc);
        }
        //calculate vel constrainted by stop distant
        double delta_s_maxacc_triangle = max_acc * t_zero_to_maxacc * t_zero_to_maxacc;
        if (delta_s_maxacc_triangle >= stat.config.safety.safetyCfg.stoppingDist)
        {
            //no const acc
            double temp_val = stat.config.safety.safetyCfg.stoppingDist / jerk;
            v_dist_limit = jerk * pow(temp_val, 2.0 / 3.0);
        }
        else
        {
            //has const acc
            double v_1 = 0.5 * jerk * t_zero_to_maxacc * t_zero_to_maxacc;
            double coef_a = 0.5 * max_acc;
            double coef_b = v_1 + max_acc * t_zero_to_maxacc;
            double coef_c = 0.5 * max_acc * t_zero_to_maxacc * t_zero_to_maxacc + v_1 * t_zero_to_maxacc - stat.config.safety.safetyCfg.stoppingDist;
            double delta_val = coef_b * coef_b - 4 * coef_a * coef_c;
            if (delta_val < 0.0)
            {
                //无解，按照三角算
                v_dist_limit = max_acc * max_acc / jerk;
            }
            else
            {
                double t_2 = (-coef_b + sqrt(delta_val)) / (2 * coef_a);
                if (t_2 < 0.0)
                {
                    v_dist_limit = max_acc * max_acc / jerk;
                }
                else
                {
                    v_dist_limit = max_acc * t_2 + v_1;
                }
            }
        }
    }
    // MOTAL_INFO(" ======================= >> v_dist_limit = %f, v_time_limit = %f\n", v_dist_limit, v_time_limit);
    ret_vel = fmin(v_dist_limit, v_time_limit);
    return ret_vel;
}

int RobotProxy::zucTrajJointMove(ZucPose end,
                                 int type,
                                 double velPercent,
                                 double ini_maxvel,
                                 double acc,
                                 double jerk,
                                 int indexrotary,
                                 int di_type,
                                 int di_index,
                                 int di_state,
                                 int jointNum,
                                 int planner_type)  //motionflag  添加关节运动
{
    if (stat.protective_stop == 1 && stat_robotproxy.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do joint move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajJointMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    int id = stat.config.TrajConfig.MotionId;
    zuc_traj_jointmov_t trajjointmov = {};
    trajjointmov.pos = end;  // Note that, here the position is in Joint Space
    trajjointmov.motion_type = type;
    trajjointmov.vel = velPercent;
    trajjointmov.ini_maxvel = ini_maxvel;
    trajjointmov.acc = acc;
    trajjointmov.turn = indexrotary;
    trajjointmov.arcBlendEnable = 0; /*added here, by zxqi to avoid blending*/
    trajjointmov.jerk = jerk;
    trajjointmov.di_type = di_type;
    trajjointmov.di_state = di_state;
    trajjointmov.di_index = di_index;
    trajjointmov.planner_type = planner_type;

    if ((!stat.userset_use_toppra || TOPP.kinematic_error) && trajjointmov.planner_type == TOPP_PLANNER)
    {
        MOTAL_INFO("******************************** [unable use toppra] ---> planner_type = T_PLANNER "
                   "*******************************\n");
        trajjointmov.planner_type = T_PLANNER;  //配置文件中没有开启时间最优或整圆运动时默认使用T规划
    }
    if (trajjointmov.planner_type == S_PLANNER && trajjointmov.jerk < 1e-6)
    {
        trajjointmov.jerk = 0.0;
        trajjointmov.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }

    int motion_type = TC_JOINT;
    double safety_func_vel = get_vel_by_stopdisttime(jerk, motion_type, trajjointmov.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajjointmov.vel = safety_func_vel;
        trajjointmov.ini_maxvel = safety_func_vel;
    }
    // clock_t start_t, finish_t;
    // double duration;
    //TOPPRA_TODO : 不使用toppra运动段也要对last_tc_goal_joint和转接半径进行更新
    // init_DYN();
    if (TOPP.first_command)
    {
        get_last_pos();
        TOPP.first_command = 0;
    }

    if (trajjointmov.planner_type == TOPP_PLANNER)
    {
        // MOTAL_INFO(" **************************** start movj_get_toppra_params, jointNum = %d, end = %f %f %f %f %f %f\n", jointNum, end.tran.x, end.tran.y, end.tran.z, end.a, end.b, end.c);
        // csaps::DoubleArray xidata(5);
        // xidata << DoubleArray::LinSpaced(5, 0, 1);
        // MOTAL_INFO("xidata = %f %f %f %f %f\n", xidata(0), xidata(1), xidata(2), xidata(3), xidata(4));
        // start_t = clock();  //计时
        if (movj_get_toppra_params() != 0)
        {
            trajjointmov.planner_type = T_PLANNER;
            MOTAL_INFO("[movj_get_toppra_params != 0]   --->   [planner_type = T_PLANNER]\n");
        }
        // finish_t = clock();
        // duration = (double)(finish_t - start_t) / CLOCKS_PER_SEC;
        // MOTAL_INFO(
        //     " ****************************************************** movj_get_toppra_params = %fms ****************************************************** \n",
        //     duration * 1000);

        // MOTAL_INFO("end movj_get_toppra_params\n");
        /* update last_tc_goal_jointpos and last_tc_blend_r */
        // MOTAL_INFO("before update, last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3], last_tc_goal_jointpos[4],
        //                 last_tc_goal_jointpos[5]);
        if (!stat.motion.already_inpos)
        {
            update_related_params();
        }
        else
        {
            TOPP.cur_tc_blend_r = 0.0;
            TOPP.last_tc_blend_r = 0.0;
            MOTAL_INFO(" --------------------------------------------------- stat.motion.already_inpos --------------------------------------------------- \n");
            // return 0;
        }
        // MOTAL_INFO("after update, last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3], last_tc_goal_jointpos[4],
        //                 last_tc_goal_jointpos[5]);
    }
    // MOTAL_INFO(" ============================================= Finish zucTrajJointMove ============================================= \n");
    if (trajjointmov.planner_type != TOPP_PLANNER)
    {
        stat_robotproxy.cur_tc_goal_jointpos[0] = end.tran.x;
        stat_robotproxy.cur_tc_goal_jointpos[1] = end.tran.y;
        stat_robotproxy.cur_tc_goal_jointpos[2] = end.tran.z;
        stat_robotproxy.cur_tc_goal_jointpos[3] = end.a;
        stat_robotproxy.cur_tc_goal_jointpos[4] = end.b;
        stat_robotproxy.cur_tc_goal_jointpos[5] = end.c;
        update_related_params2();
    }
    TOPP.kinematic_error = 0;

    auto cmd = std::make_shared<CmdSetJointLine>(stat.rob_id, id, motion_type, trajjointmov);
    return mot::realtime_cmd_call(cmd);
}

void RobotProxy::update_related_params()  //motionflag 更新轨迹参数
{
    TOPP.last_tc_blend_r = TOPP.cur_tc_blend_r;
    // lasttc_jointNum = zucmotCommand.joint;  // 没用
    int index = 0;
    for (int i = 0; i < stat.robot_joint_num; i++)
    {
        TOPP.last_tc_goal_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        TOPP.last_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_blend_jointpos[i];

        if (TOPP.cur_tc_blend_r > SMALL)
        {
            for (int j = REFIT_NUM; j > 0; j--)
            {
                index = TOPP.cur_blend_len - j;
                TOPP.last_tc_refit_jpos[REFIT_NUM - j][i] = TOPP.path_info[index].pathq_coefs[i][3];
            }
        }
        else
        {
            for (int j = REFIT_NUM; j > 0; j--) { TOPP.last_tc_refit_jpos[REFIT_NUM - j][i] = stat_robotproxy.cur_tc_blend_jointpos[i]; }
        }

        TOPP.last_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_blend_jointpos_sec[i];
    }
    index = TOPP.cur_blend_len;  // toppra changed here
}

void RobotProxy::update_related_params2()  //motionflag 更新轨迹参数
{
    TOPP.last_tc_blend_r = 0.0;
    TOPP.cur_tc_blend_r = 0.0;
    TOPP.last_final_pathp = 0.0;
    TOPP.lasttc_left_len = 0;
    // lasttc_jointNum = zucmotCommand.joint;

    for (int i = 0; i < stat.robot_joint_num; i++)
    {
        TOPP.last_tc_goal_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        TOPP.last_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        TOPP.last_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];

        for (int j = REFIT_NUM; j > 0; j--) { TOPP.last_tc_refit_jpos[REFIT_NUM - j][i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
    }
}

void RobotProxy::get_last_pos()
{
    // MOTAL_INFO(" ************************************** [first_command] ************************************** \n");
    // usrmotReadZucmotStatus(&zucmotStatus);
    for (int i = 0; i < stat.robot_joint_num; i++)
    {
        // TOPP.last_tc_goal_jointpos[i] = zucmotStatus.joint_status[i].pos_cmd;
        TOPP.last_tc_blend_jointpos[i] = TOPP.last_tc_goal_jointpos[i];
        TOPP.last_tc_blend_jointpos_sec[i] = TOPP.last_tc_goal_jointpos[i];
        for (int j = 0; j < REFIT_NUM; j++) { TOPP.last_tc_refit_jpos[j][i] = stat.motion.jog.posCmd[i]; }
        // rcs_print("first_command: joint %d last_tc_refit_jpos = %f %f %f %f %f %f\n", i, last_tc_refit_jpos[0][i], last_tc_refit_jpos[1][i],
        //               last_tc_refit_jpos[2][i], last_tc_refit_jpos[3][i], last_tc_refit_jpos[4][i], last_tc_refit_jpos[5][i]);

        stat_robotproxy.cur_tc_goal_jointpos[i] = TOPP.last_tc_goal_jointpos[i];  // TOPPRA_TODO : is this ok for joint move?
        stat_robotproxy.cur_tc_blend_jointpos[i] = TOPP.last_tc_goal_jointpos[i];
        stat_robotproxy.cur_tc_blend_jointpos_sec[i] = TOPP.last_tc_goal_jointpos[i];
    }
    // for(int j = 0; j < REFIT_NUM; j ++){
    //     rcs_print("last_tc_refit_num %d = %f %f %f %f %f %f\n", j, last_tc_refit_jpos[j][0], last_tc_refit_jpos[j][1], last_tc_refit_jpos[j][2],
    //     last_tc_refit_jpos[j][3], last_tc_refit_jpos[j][4], last_tc_refit_jpos[j][5]);
    // }
    TOPP.last_tc_blend_r = 0.0;
    TOPP.last_final_pathp = 0.0;
    TOPP.lasttc_jointNum = 0;
    // MOTAL_INFO("first_command ---> last_tc_goal_jointpos = %f %f %f %f %f %f\n",
    //             last_tc_goal_jointpos[0],
    //             last_tc_goal_jointpos[1],
    //             last_tc_goal_jointpos[2],
    //             last_tc_goal_jointpos[3],
    //             last_tc_goal_jointpos[4],
    //             last_tc_goal_jointpos[5]);
}

int TOPP_member::get_toppra_related_params(const int discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)  //motionflag  toppuse 更新topp相关参数
{
    blend_jpos_num = lasttc_left_len + cur_last_blend_len;
    // MOTAL_INFO("lasttc_left_len = %d, cur_last_blend_len = %d, blend_jpos_num = %d\n", lasttc_left_len, TOPP.cur_last_blend_len,
    // zucmotCommand.blend_jpos_num);
    jpos_len = discrete_num;
    /* start to fit with csaps */

    double start_path_data = last_final_pathp;
    // get_pathvalue_coefs_pycubic(discrete_num, jpos_for_fit);  //disableflag 先注释 ybh need fix

    // ConstraintsToppra cons_toppra;
    // zucmotCommand.cons_toppra.req_vel[0] = 0;
    set_constraints_toppra(cons_toppra);
    // MOTAL_INFO("cons_toppra.vel_lim = %f %f\n", zucmotCommand.cons_toppra.vel_lim[0][0], zucmotCommand.cons_toppra.vel_lim[0][1]);
    get_path_information(discrete_num, cons_toppra);

    if ((last_tc_blend_r > SMALL) && (cur_last_blend_len > 0) && (blend_jpos_num > 6))  // TOPPRA_TODO: >6 ??
    {
        /* 对转接段进行拟合,计算转接段的系数 */
        // MOTAL_INFO(" ========================== last_tc_blend_r = %f,  TOPP.cur_last_blend_len = %d,  zucmotCommand.blend_jpos_num = %d "
        //             "========================== \n",
        //             last_tc_blend_r,
        //             TOPP.cur_last_blend_len,
        //             zucmotCommand.blend_jpos_num);
        get_blend_pathvalue_coefs_csaps(blend_jpos_num, start_path_data, blend_jpos_for_fit);
        get_blend_path_information(blend_jpos_num + 2 + REFIT_NUM, cons_toppra);
        // MOTAL_INFO(" ========================== finish get blend pos, blend_jpos_num = %d ========================== \n", zucmotCommand.blend_jpos_num);
    }
    else
    {
        cur_last_blend_len = 0;
        cur_blend_len = discrete_num - 1;
    }
    // MOTAL_INFO(
    //     " =================>>>>>>>>>>>>>>>>>>>>>>>>> TOPP.cur_blend_len = %d, discrete_num = %d \n", TOPP.cur_blend_len, discrete_num);
    lasttc_left_len = discrete_num - cur_blend_len;  // update lasttc_left_len;
    cur_blend_left = lasttc_left_len;
    return 0;
}

int TOPP_member::zucTrajGetFilterSize(int filter_size)  //motionflag topp会用
{
    userset_filter_size = filter_size;
    MOTAL_INFO(" ************************************************** userset_filter_size = %d ************************************************** \n",
               userset_filter_size);
    return 0;
}

int TOPP_member::zucTrajGetAddTorCons(int add_tor_cons)  //motionflag topp会用  看起来没用到
{
    useradd_tor_cons = add_tor_cons;
    MOTAL_INFO(" ************************************************** useradd_tor_cons = %d ************************************************** \n",
               useradd_tor_cons);
    return 0;
}

int TOPP_member::zucTrajGetTorPortion(double tor_portion)  //motionflag topp会用 力矩限制系数？
{
    usertor_portion = tor_portion;
    //为避免影响到动力学相关算法，设置比例进行关节力矩限制范围的缩小
    if (usertor_portion > 1.0)
    {
        usertor_portion = 1.0;
    }
    else if (usertor_portion < 0.5)
    {
        usertor_portion = 0.5;
    }
    return 0;
}

int TOPP_member::zucTrajSetDisJLen(double dislen)  //motionflag topp会用 关节最小离散距离
{
    // TOPP.disjlen = dislen;  //应该不用在command里
    disjlen = dislen;
    MOTAL_INFO(" ************************************************** TOPP.disjlen = %f ************************************************** \n", disjlen);
    return 0;
}

int TOPP_member::zucTrajSetDisLLen(double dislen)  //motionflag topp会用 直线最小离散距离
{
    // zucmotCommand.disllen = dislen;
    disllen = dislen;
    MOTAL_INFO(" ************************************************** zucmotCommand.disllen = %f ************************************************** \n", disllen);
    return 0;
}

int TOPP_member::zucTrajSetDisCLen(double dislen)  //motionflag topp会用 圆弧最小离散距离
{
    // TOPP.disclen = dislen;
    disclen = dislen;
    MOTAL_INFO(" ************************************************** TOPP.disclen = %f ************************************************** \n", disclen);
    return 0;
}

int TOPP_member::zucTrajSetSmoothPara(double smooth_para)  //motionflag topp会用 平滑参数？
{
    // zucmotCommand.smooth_para = smooth_para;
    smooth_para = smooth_para;
    MOTAL_INFO(" ************************************************** zucmotCommand.smooth_para = %f ************************************************** \n",
               smooth_para);
    return 0;
}

int RobotProxy::zucTrajSetRateBlendPara(int rate_blend, int topp_queue_len, int max_fw_len)  //motionflag topp会用 与调整倍率有关
{
    auto cmd = std::make_shared<CmdSetRateBlend>(stat.rob_id, rate_blend, topp_queue_len, max_fw_len);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucToppraFinishLeftTc(void)  //motionflag topp相关
{
    // MOTAL_INFO("----------zucToppraFinishLeftTc----------:    \n");
    TOPP.first_command = 1;
    TOPP.kinematic_error = 0;

    auto cmd = std::make_shared<CmdToppraFinishLeftTc>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucToppraFirstCommand(void)  //motionflag topp相关
{
    // MOTAL_INFO(" ---------- zucToppraFirstCommand ---------- :    \n");
    TOPP.first_command = 1;
    TOPP.kinematic_error = 0;
    TOPP.lasttc_left_len = 0;
    if (TOPP.blend_seg_jpos.jpos != NULL)
    {
        TOPP.blend_seg_jpos.rear = 0;
        TOPP.blend_seg_jpos.front = 0;
    }
    else
    {
        MOTAL_INFO(" ************ blend_seg_jpos == NULL ************ \n");
    }

    auto cmd = std::make_shared<CmdToppraFirstCommand>(stat.rob_id);
    return mot::realtime_cmd_call(cmd);
}

void TOPP_member::get_lasttc_blend_jpos(ZucJPose* blend_jpos_for_fit)  //motionfalg toppused 更新topp所需转接关节点信息
{
    BlendJpos blendjpos;
    int get_front_ret = -1;
    for (int i = 0; i < lasttc_left_len; i++)
    {
        get_front_ret = GetBlendFront(&blendjpos);
        if (get_front_ret != 0)
        {
            last_tc_blend_r = 0;
            return;
        }
        else
        {
            for (int j = 0; j < robot_joint_num; j++) { blend_jpos_for_fit[i].jpos[j] = blendjpos.blendjpos[j]; }
        }
    }
}

int TOPP_member::movj_get_toppra_params()  //motionflag topp相关 关节运动参数
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置, start from 0;
    cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];
    if (last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = get_joint_discrete_info(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

//motionflag topp相关 直线运动参数
int RobotProxy::movl_get_toppra_params(ZucPose start_cartpos, KINEMATICS_FORWARD_FLAGS* toppra_fflags, KINEMATICS_INVERSE_FLAGS* toppra_iflags, double tmag_abc)
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    TOPP.cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置,start from 0;
    TOPP.cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];

    if (TOPP.last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (TOPP.blend_seg_jpos.jpos != NULL)
    {
        TOPP.blend_seg_jpos.rear = 0;
        TOPP.blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = 0;
    if (!stat_robotproxy.trajlinearmov.using_abc_vel)
    {
        discrete_ret = get_movl_discrete_info(start_cartpos, discrete_num, jpos_for_fit, blend_jpos_for_fit, toppra_fflags, toppra_iflags);
    }
    else
    {
        discrete_ret = get_movl_discrete_info_abc(start_cartpos, discrete_num, jpos_for_fit, blend_jpos_for_fit, toppra_fflags, toppra_iflags, tmag_abc);
    }

    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        MOTAL_INFO(" ************** [movl_get_toppra_params] ---> discrete_ret != 0 ************** \n");
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

//motionflag topp相关 圆弧运动参数
int TOPP_member::movc_get_toppra_params()
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置,start from 0;
    cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];

    if (last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = 0;
    discrete_ret = get_movc_discrete_info(discrete_num, jpos_for_fit, blend_jpos_for_fit);

    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        MOTAL_INFO(" ************** [movc_get_toppra_params] ---> discrete_ret != 0 ************** \n");
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

//motionflag topp相关 关节离散信息
int RobotProxy::get_joint_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();

    int i = 0;
    double progress = 0.0, tar_in_jpos, tar_in_cartpos, delta_x, delta_y, delta_z;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos, last_tc_goal_cartpos, cur_tc_goal_cartpos;
    KINEMATICS_FORWARD_FLAGS toppra_fflags = 0;
    KINEMATICS_INVERSE_FLAGS toppra_iflags = 0;
    /* update cur_tc_goal_jointpos */
    stat_robotproxy.cur_tc_goal_jointpos[0] = stat_robotproxy.trajjointmov.pos.tran.x;
    stat_robotproxy.cur_tc_goal_jointpos[1] = stat_robotproxy.trajjointmov.pos.tran.y;
    stat_robotproxy.cur_tc_goal_jointpos[2] = stat_robotproxy.trajjointmov.pos.tran.z;
    stat_robotproxy.cur_tc_goal_jointpos[3] = stat_robotproxy.trajjointmov.pos.a;
    stat_robotproxy.cur_tc_goal_jointpos[4] = stat_robotproxy.trajjointmov.pos.b;
    stat_robotproxy.cur_tc_goal_jointpos[5] = stat_robotproxy.trajjointmov.pos.c;

    // MOTAL_INFO("[get_joint_discrete_info] ---> last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3],
    //                 last_tc_goal_jointpos[4], last_tc_goal_jointpos[5]);
    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    // kinematicsForward(&(zucmotConfig.tool_offset),
    //                   last_tc_goal_jointpos,
    //                   &last_tc_goal_cartpos,
    //                   &toppra_fflags,
    //                   &toppra_iflags,
    //                   &(zucmotConfig.base_offset),
    //                   &(zucmotConfig.user_offset));
    // kinematicsForward(&(zucmotConfig.tool_offset),
    //                   cur_tc_goal_jointpos,
    //                   &cur_tc_goal_cartpos,
    //                   &toppra_fflags,
    //                   &toppra_iflags,
    //                   &(zucmotConfig.base_offset),
    //                   &(zucmotConfig.user_offset));
    kine->fkine(RobJointVal(TOPP.last_tc_goal_jointpos), last_tc_goal_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
    kine->fkine(RobJointVal(stat_robotproxy.cur_tc_goal_jointpos), cur_tc_goal_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);

    delta_x = cur_tc_goal_cartpos.tran.x - last_tc_goal_cartpos.tran.x;
    delta_y = cur_tc_goal_cartpos.tran.y - last_tc_goal_cartpos.tran.y;
    delta_z = cur_tc_goal_cartpos.tran.z - last_tc_goal_cartpos.tran.z;
    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // MOTAL_INFO(" ------------------ tar_in_cartpos = %f ------------------\n", tar_in_cartpos);
    if (TOPP.cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (TOPP.last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    int jointNum = stat.joint;

    // MOTAL_INFO("jointNum = %d, cur_tc_goal_jointpos[jointNum] = %f, last_tc_goal_jointpos[jointNum] = %f\n", jointNum, cur_tc_goal_jointpos[jointNum], last_tc_goal_jointpos[jointNum]);
    tar_in_jpos = fabs(stat_robotproxy.cur_tc_goal_jointpos[jointNum] - TOPP.last_tc_goal_jointpos[jointNum]);  //如果有笛卡尔空间运动就不能这么计算
    // MOTAL_INFO(" ######################################## [get_joint_discrete_info] ---> tar_in_jpos = %f\n", tar_in_jpos);
    if (tar_in_jpos < SMALL)
    {  //应该不会出现这种情况
        MOTAL_INFO("tar_in_jpos == 0.0, cur_tc_goal_jointpos[jointNum] = %f\n", stat_robotproxy.cur_tc_goal_jointpos[jointNum]);
        stat.motion.already_inpos = 1;
        TOPP.cur_tc_blend_r = 0.0;
        TOPP.last_final_pathp = 0.0;
        for (int i = 0; i < stat.robot_joint_num; i++)
        {
            stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
            stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        }
        MOTAL_INFO("[get_joint_discrete_info]  --->  already in position\n");
        return 1;
    }
    else
    {
        stat.motion.already_inpos = 0;
    }

    double increment = tar_in_jpos / (MAX_POINT_NUM - 1);
    if ((TOPP.disjlen <= 0) || (TOPP.disjlen > 1.5))
    {
        TOPP.disjlen = DISCRETE_JOINT_LENGTH;
    }

    if (increment < TOPP.disjlen)
    {
        increment = TOPP.disjlen;
    }
    if ((tar_in_jpos / increment) < 20)
    {
        // increment = tar_in_jpos / 4;
        MOTAL_INFO("[get_movj_discrete_info]: too short line\n");
        return 1;
    }

    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (TOPP.last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
            stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        }
    }

    discrete_num = 0;
    progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    while (progress <= tar_in_jpos)
    {
        if (fabs(progress - tar_in_jpos) < SMALL)
        {
            progress = tar_in_jpos;
        }

        for (i = 0; i < stat.robot_joint_num; i++)
        {
            jpos_for_fit[discrete_num].jpos[i] =
                TOPP.last_tc_goal_jointpos[i] + progress / tar_in_jpos * (stat_robotproxy.cur_tc_goal_jointpos[i] - TOPP.last_tc_goal_jointpos[i]);
        }
        // kinematicsForward(&(zucmotConfig.tool_offset),
        //                   jpos_for_fit[discrete_num].jpos,
        //                   &discrete_cartpos,
        //                   &toppra_fflags,
        //                   &toppra_iflags,
        //                   &(zucmotConfig.base_offset),
        //                   &(zucmotConfig.user_offset));
        kine->fkine(RobJointVal(jpos_for_fit[discrete_num].jpos), discrete_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);

        if (shorter_than_last_blend_r)
        {
            delta_x = discrete_cartpos.tran.x - last_tc_goal_cartpos.tran.x;
            delta_y = discrete_cartpos.tran.y - last_tc_goal_cartpos.tran.y;
            delta_z = discrete_cartpos.tran.z - last_tc_goal_cartpos.tran.z;
            len_from_last_goalp = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
            if (len_from_last_goalp >= TOPP.last_tc_blend_r)
            {
                TOPP.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }

            if (progress != 0.0)
            {
                for (int i = 0; i < stat.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[TOPP.lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            delta_x = discrete_cartpos.tran.x - cur_tc_goal_cartpos.tran.x;
            delta_y = discrete_cartpos.tran.y - cur_tc_goal_cartpos.tran.y;
            delta_z = discrete_cartpos.tran.z - cur_tc_goal_cartpos.tran.z;
            len_from_cur_goalp = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
            if (len_from_cur_goalp <= TOPP.cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
                // MOTAL_INFO("************* cur_blend_pos = %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
                //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
            }
            TOPP.cur_blend_len = discrete_num;
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        if ((TOPP.cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                TOPP.cur_tc_blend_r = 0;
                TOPP.cur_blend_len = 0;
            }
        }

        // if (progress == 0.0)
        // {
        TOPP.path_info[0].min_time = 0.0;
        // }
        // else
        // {
        //     TOPP.path_info[discrete_num].min_time =
        //         fabs(jpos_for_fit[discrete_num].jpos[zucmotCommand.joint] - jpos_for_fit[discrete_num - 1].jpos[zucmotCommand.joint]) /
        //         trajjointmov.ini_maxvel;
        // }
        discrete_num += 1;
        if (progress == tar_in_jpos)
        {
            if (TOPP.cur_tc_blend_r <= SMALL)
            {
                TOPP.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        progress += increment;
        if (progress > tar_in_jpos)
        {
            progress = tar_in_jpos;
        }
    }

    return 0;
}

//motionflag topp相关 获取切线方向
int TOPP_member::get_unit_vector(double* vector, double mag, double* unit_vector)
{
    int i = 0;
    if (mag > CLOSE_TO_ZERO)
    {
        for (i = 0; i < 3; i++) { unit_vector[i] = vector[i] / mag; }
    }
    else
    {
        for (i = 0; i < 3; i++) { unit_vector[i] = 0.0; }
    }
    return 0;
}

//motionflag topp相关 直线轨迹点信息
int RobotProxy::get_movl_discrete_info(ZucPose start_cartpos,
                                       int& discrete_num,
                                       ZucJPose* jpos_for_fit,
                                       ZucJPose* blend_jpos_for_fit,
                                       KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                       KINEMATICS_INVERSE_FLAGS* toppra_iflags)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();

    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos, delta_x, delta_y, delta_z;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos;
    double dist_vector[3] = {0}, dist_unit_vector[3] = {0};

    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    delta_x = stat_robotproxy.trajlinearmov.pos.tran.x - start_cartpos.tran.x;
    delta_y = stat_robotproxy.trajlinearmov.pos.tran.y - start_cartpos.tran.y;
    delta_z = stat_robotproxy.trajlinearmov.pos.tran.z - start_cartpos.tran.z;

    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // MOTAL_INFO("delta_x/y/z = %f, %f, %f, tar_in_cartpos = %f\n", delta_x, delta_y, delta_z, tar_in_cartpos);
    // tar_in_cartpos too small

    dist_vector[0] = delta_x;
    dist_vector[1] = delta_y;
    dist_vector[2] = delta_z;
    get_unit_vector(dist_vector, tar_in_cartpos, dist_unit_vector);
    if (TOPP.cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (TOPP.last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    if (tar_in_cartpos < SMALL)
    {  //应该不会出现这种情况
        stat.motion.already_inpos = 1;
        TOPP.cur_tc_blend_r = 0.0;
        TOPP.last_final_pathp = 0.0;
        for (int i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
        MOTAL_INFO("[get_movl_discrete_info]  --->  already in position\n");
        return 1;
    }
    else
    {
        stat.motion.already_inpos = 0;
    }

    double increment = tar_in_cartpos / (MAX_POINT_NUM - 1);
    if ((TOPP.disllen <= 0) || (TOPP.disllen > 5))
    {
        TOPP.disllen = DISCRETE_CART_LENGTH;
    }

    if (increment < TOPP.disllen)
    {
        increment = TOPP.disllen;
    }
    if ((tar_in_cartpos / increment) < 20)
    {
        MOTAL_INFO("[get_movl_discrete_info]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }
    MOTAL_INFO(" ----------------------------------------------------- [get_movl_discrete_info] increment = %f\n", increment);
    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (TOPP.last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    double linearvel = stat_robotproxy.trajlinearmov.ini_maxvel;
    if (linearvel > TOPP.tcp_vel_limit_toppra)
    {
        linearvel = TOPP.tcp_vel_limit_toppra;
    }
    while (progress <= tar_in_cartpos)
    {
        if (fabs(progress - tar_in_cartpos) < SMALL)
        {
            progress = tar_in_cartpos;
        }
        get_line_position(&start_cartpos, dist_unit_vector, progress, &discrete_cartpos);
        get_line_gesture(&start_cartpos, &stat_robotproxy.trajlinearmov.pos, (progress / tar_in_cartpos), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
            TOPP.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            // int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
            //                                 &discrete_cartpos,
            //                                 jpos_for_fit[discrete_num].jpos,
            //                                 toppra_iflags,
            //                                 toppra_fflags,
            //                                 &zucmotConfig.base_offset,
            //                                 &zucmotConfig.user_offset);
            kine::KineItf::kine_Conf toppra_conf;
            RobJointVal jpos(jpos_for_fit[discrete_num].jpos);
            // kine::KineItf::kine_Conf conf;
            auto inv_ret = kine->ikine(discrete_cartpos, jpos, toppra_conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
            jpos.array_value(jpos_for_fit[discrete_num].jpos);
            if (inv_ret != 0)
            {
                MOTAL_INFO(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            // TOPP.path_info[discrete_num].min_time = fabs(progress - last_progress) / linearvel;
        }
        // MOTAL_INFO("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y, discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // MOTAL_INFO(" **************************************** %dth %f %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);

        if (shorter_than_last_blend_r)
        {
            if (progress >= TOPP.last_tc_blend_r)
            {
                TOPP.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (progress != 0.0)
            {
                for (int i = 0; i < stat.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[TOPP.lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }
            // MOTAL_INFO("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos - progress) <= TOPP.cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            TOPP.cur_blend_len = discrete_num;
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(TOPP.cur_blend_len != discrete_num){
        //     MOTAL_INFO("*********************************************************** TOPP.cur_blend_len = %d, cur_tc_blend_jointpos = %f %f %f %f %f %f\n", TOPP.cur_blend_len,
        //             cur_tc_blend_jointpos[0], cur_tc_blend_jointpos[1], cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4], cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && cur_tc_blend_r > SMALL){
        //     MOTAL_INFO("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((TOPP.cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                TOPP.cur_tc_blend_r = 0;
                TOPP.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tar_in_cartpos)
        {
            if (TOPP.cur_tc_blend_r <= SMALL)
            {
                TOPP.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tar_in_cartpos)
        {
            progress = tar_in_cartpos;
        }
    }

    /* update cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        stat_robotproxy.cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        stat_robotproxy.cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        stat_robotproxy.cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        stat_robotproxy.cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        stat_robotproxy.cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        stat_robotproxy.cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
            stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        }
    }
    return 0;
}

//motionflag topp相关 直线姿态点信息
int RobotProxy::get_movl_discrete_info_abc(ZucPose start_cartpos,
                                           int& discrete_num,
                                           ZucJPose* jpos_for_fit,
                                           ZucJPose* blend_jpos_for_fit,
                                           KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                                           KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                                           double tmag_abc)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();

    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos = 0.0, tar_in_cart_abc = 0.0, delta_x = 0.0, delta_y = 0.0,
           delta_z = 0.0;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos;
    double dist_vector[3] = {0}, dist_unit_vector[3] = {0};

    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    delta_x = stat_robotproxy.trajlinearmov.pos.tran.x - start_cartpos.tran.x;
    delta_y = stat_robotproxy.trajlinearmov.pos.tran.y - start_cartpos.tran.y;
    delta_z = stat_robotproxy.trajlinearmov.pos.tran.z - start_cartpos.tran.z;

    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // MOTAL_INFO("delta_x/y/z = %f, %f, %f, tar_in_cartpos = %f\n", delta_x, delta_y, delta_z, tar_in_cartpos);
    // tar_in_cartpos too small

    dist_vector[0] = delta_x;
    dist_vector[1] = delta_y;
    dist_vector[2] = delta_z;
    get_unit_vector(dist_vector, tar_in_cartpos, dist_unit_vector);
    // MOTAL_INFO("[get_movl_discrete_info_abc] ---> start_cartpos = %f %f %f %f %f %f, end_pos = %f %f %f %f %f %f, delta = %f %f %f, dist_unit_vector = %f %f %f\n",
    //                     start_cartpos.tran.x, start_cartpos.tran.y, start_cartpos.tran.z, start_cartpos.a,
    //                     start_cartpos.b, start_cartpos.c, stat_robotproxy.trajlinearmov.pos.tran.x, stat_robotproxy.trajlinearmov.pos.tran.y, stat_robotproxy.trajlinearmov.pos.tran.z,
    //                     stat_robotproxy.trajlinearmov.pos.a, stat_robotproxy.trajlinearmov.pos.b, stat_robotproxy.trajlinearmov.pos.c,
    //                     dist_vector[0], dist_vector[1], dist_vector[2], dist_unit_vector[0], dist_unit_vector[1], dist_unit_vector[2]);

    if (TOPP.cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (TOPP.last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        TOPP.last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    if (tar_in_cartpos < SMALL && tmag_abc < SMALL)
    {  //应该不会出现这种情况
        stat.motion.already_inpos = 1;
        TOPP.cur_tc_blend_r = 0.0;
        TOPP.last_final_pathp = 0.0;
        for (int i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
        MOTAL_INFO("[get_movl_discrete_info_abc]  --->  already in position\n");
        return 1;
    }
    else
    {
        stat.motion.already_inpos = 0;
    }

    double increment = tmag_abc / (MAX_POINT_NUM - 1);
    if ((TOPP.disjlen <= 0) || (TOPP.disjlen > 1.5))
    {
        TOPP.disjlen = DISCRETE_CART_LENGTH;
    }

    if (increment < TOPP.disjlen)
    {
        increment = TOPP.disjlen;
    }
    if ((tmag_abc / increment) < 20)
    {
        MOTAL_INFO("[get_movl_discrete_info_abc]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }
    MOTAL_INFO(" ----------------------------------------------------- [get_movl_discrete_info_abc] increment = %f\n", increment);
    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (TOPP.last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    double dist_from_start = 0.0;
    while (progress <= tmag_abc)
    {
        if (fabs(progress - tmag_abc) < SMALL)
        {
            progress = tmag_abc;
        }
        dist_from_start = progress / tmag_abc * tar_in_cartpos;
        get_line_position(&start_cartpos, dist_unit_vector, dist_from_start, &discrete_cartpos);
        get_line_gesture(&start_cartpos, &stat_robotproxy.trajlinearmov.pos, (progress / tmag_abc), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
            TOPP.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            // int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
            //                                 &discrete_cartpos,
            //                                 jpos_for_fit[discrete_num].jpos,
            //                                 toppra_iflags,
            //                                 toppra_fflags,
            //                                 &zucmotConfig.base_offset,
            //                                 &zucmotConfig.user_offset);
            kine::KineItf::kine_Conf toppra_conf;
            RobJointVal jpos(jpos_for_fit[discrete_num].jpos);
            // kine::KineItf::kine_Conf conf;
            auto inv_ret = kine->ikine(discrete_cartpos, jpos, toppra_conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
            jpos.array_value(jpos_for_fit[discrete_num].jpos);
            if (inv_ret != 0)
            {
                MOTAL_INFO(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            TOPP.path_info[discrete_num].min_time = fabs(progress - last_progress) / stat_robotproxy.trajlinearmov.ori_vel;  // attention here, the velocity
            // MOTAL_INFO(" --- --- --- --- %dth min_time = %f \n", discrete_num, TOPP.path_info[discrete_num].min_time);
        }
        // MOTAL_INFO("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y, discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // MOTAL_INFO(" **************************************** %dth %f %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // delta_x = discrete_cartpos.tran.x - start_cartpos.tran.x;
        // delta_y = discrete_cartpos.tran.y - start_cartpos.tran.y;
        // delta_z = discrete_cartpos.tran.z - start_cartpos.tran.z;
        // dist_from_start = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));

        if (shorter_than_last_blend_r)
        {
            if (dist_from_start >= TOPP.last_tc_blend_r)
            {
                TOPP.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (dist_from_start != 0.0)
            {
                for (int i = 0; i < stat.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[TOPP.lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }  //TODO
            }
            // MOTAL_INFO("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos - dist_from_start) <= TOPP.cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            TOPP.cur_blend_len = discrete_num;
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(TOPP.cur_blend_len != discrete_num){
        //     MOTAL_INFO("*********************************************************** TOPP.cur_blend_len = %d, stat_robotproxy.cur_tc_blend_jointpos = %f %f %f %f %f %f\n", TOPP.cur_blend_len,
        //             stat_robotproxy.cur_tc_blend_jointpos[0], stat_robotproxy.cur_tc_blend_jointpos[1], stat_robotproxy.cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4], cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && TOPP.cur_tc_blend_r > SMALL){
        //     MOTAL_INFO("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((TOPP.cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }  //*******TODO
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                TOPP.cur_tc_blend_r = 0;
                TOPP.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tmag_abc)
        {
            if (TOPP.cur_tc_blend_r <= SMALL)
            {
                TOPP.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tmag_abc)
        {
            progress = tmag_abc;
        }
    }

    /* update stat_robotproxy.cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        stat_robotproxy.cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        stat_robotproxy.cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        stat_robotproxy.cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        stat_robotproxy.cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        stat_robotproxy.cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        stat_robotproxy.cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
            stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        }
    }
    return 0;
}

//motionflag toppuse movc获取点信息
int RobotProxy::get_movc_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();

    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos_arc, delta_x, delta_y, delta_z;  // tar_in_cartpos_arc for judge blend_r
    ZucPose start_cartpos, discrete_cartpos;
    KINEMATICS_FORWARD_FLAGS toppra_fflags = 0;
    KINEMATICS_INVERSE_FLAGS toppra_iflags = 0;
    //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
    // kinematicsForward(&(zucmotConfig.tool_offset),
    //                   stat_robotproxy.cur_tc_goal_jointpos,
    //                   &start_cartpos,
    //                   &toppra_fflags,
    //                   &toppra_iflags,
    //                   &(zucmotConfig.base_offset),
    //                   &(zucmotConfig.user_offset));
    kine->fkine(RobJointVal(stat_robotproxy.cur_tc_goal_jointpos), start_cartpos, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);

    // MOTAL_INFO("[get_movc_discrete_info] ---> stat_robotproxy.cur_tc_goal_jointpos = %f %f %f %f %f %f\n", stat_robotproxy.cur_tc_goal_jointpos[0], stat_robotproxy.cur_tc_goal_jointpos[1], stat_robotproxy.cur_tc_goal_jointpos[2], stat_robotproxy.cur_tc_goal_jointpos[3],
    //                     stat_robotproxy.cur_tc_goal_jointpos[4], stat_robotproxy.cur_tc_goal_jointpos[5]);
    // MOTAL_INFO("[get_movc_discrete_info] ---> start_cartpos = %f %f %f %f %f %f\n", start_cartpos.tran.x, start_cartpos.tran.y, start_cartpos.tran.z, start_cartpos.a,
    //                     start_cartpos.b, start_cartpos.c);

    // 确定圆心
    double sm[3] = {0}, me[3] = {0}, se[3] = {0}, sc[3] = {0};  // sc means startp-centerp
    double ms_norm = 0.0, me_norm = 0.0, se_norm = 0.0;

    sm[0] = stat_robotproxy.trajmovc.movcMid.tran.x - start_cartpos.tran.x;
    sm[1] = stat_robotproxy.trajmovc.movcMid.tran.y - start_cartpos.tran.y;
    sm[2] = stat_robotproxy.trajmovc.movcMid.tran.z - start_cartpos.tran.z;
    me[0] = stat_robotproxy.trajmovc.movcEnd.tran.x - stat_robotproxy.trajmovc.movcMid.tran.x;
    me[1] = stat_robotproxy.trajmovc.movcEnd.tran.y - stat_robotproxy.trajmovc.movcMid.tran.y;
    me[2] = stat_robotproxy.trajmovc.movcEnd.tran.z - stat_robotproxy.trajmovc.movcMid.tran.z;
    se[0] = stat_robotproxy.trajmovc.movcEnd.tran.x - start_cartpos.tran.x;
    se[1] = stat_robotproxy.trajmovc.movcEnd.tran.y - start_cartpos.tran.y;
    se[2] = stat_robotproxy.trajmovc.movcEnd.tran.z - start_cartpos.tran.z;
    ms_norm = sqrt(sm[0] * sm[0] + sm[1] * sm[1] + sm[2] * sm[2]);
    me_norm = sqrt(me[0] * me[0] + me[1] * me[1] + me[2] * me[2]);
    se_norm = sqrt(se[0] * se[0] + se[1] * se[1] + se[2] * se[2]);

    if (((ms_norm < 1e-3) || (me_norm < 1e-3)) || (se_norm < 1e-3))
    {
        //目前要求示教三点不能共线,后续若需要的话可以进行优化
        return -1;
    }
    stat.motion.already_inpos = 0;
    MovcStruct circle;  // include params needed for planning a movc
    int get_movcp_ret = get_movc_planning_params(&circle, &start_cartpos, &(stat_robotproxy.trajmovc.movcMid), &(stat_robotproxy.trajmovc.movcEnd));
    if (get_movcp_ret != 0)
    {
        return -1;
    }
    tar_in_cartpos_arc = circle.angle * circle.radius;  // length of arc
    if (TOPP.cur_tc_blend_r > tar_in_cartpos_arc * 0.45)
    {
        MOTAL_INFO("[MOVC] TOPP.cur_tc_blend_r is unsuitable, reset TOPP.cur_tc_blend_r according to target length\n");
        TOPP.cur_tc_blend_r = tar_in_cartpos_arc * 0.45;
    }
    if (TOPP.last_tc_blend_r > tar_in_cartpos_arc * 0.45)
    {
        MOTAL_INFO("[MOVC] TOPP.last_tc_blend_r is unsuitable, reset TOPP.last_tc_blend_r according to target length\n");
        TOPP.last_tc_blend_r = tar_in_cartpos_arc * 0.45;
    }
    double increment = tar_in_cartpos_arc / (MAX_POINT_NUM - 1);
    if ((TOPP.disclen <= 0) || (TOPP.disclen > 5))
    {
        TOPP.disclen = DISCRETE_CART_LENGTH;
    }
    if (increment < TOPP.disclen)
    {
        increment = TOPP.disclen;
    }
    if ((tar_in_cartpos_arc / increment) < 20)  // TOPPRA_TODO: 限制最小的离散个数，不应该是4
    {
        MOTAL_INFO("[get_movl_discrete_info]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }

    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (TOPP.last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    // TOPPRA_TODO: 以下过程和获取movl离散点的过程一致，整合一下
    while (progress <= tar_in_cartpos_arc)
    {
        if (fabs(progress - tar_in_cartpos_arc) < SMALL)
        {
            progress = tar_in_cartpos_arc;
        }
        //获取离散点的位姿
        get_movc_pos(&circle, (progress / tar_in_cartpos_arc), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i]; }
            TOPP.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < stat.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            // int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
            //                                 &discrete_cartpos,
            //                                 jpos_for_fit[discrete_num].jpos,
            //                                 &toppra_iflags,
            //                                 &toppra_fflags,
            //                                 &zucmotConfig.base_offset,
            //                                 &zucmotConfig.user_offset);
            kine::KineItf::kine_Conf toppra_conf;
            RobJointVal jpos(jpos_for_fit[discrete_num].jpos);
            // kine::KineItf::kine_Conf conf;
            auto inv_ret = kine->ikine(discrete_cartpos, jpos, toppra_conf, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
            jpos.array_value(jpos_for_fit[discrete_num].jpos);
            if (inv_ret != 0)
            {
                MOTAL_INFO(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            // TOPP.path_info[discrete_num].min_time = fabs(progress - last_progress) / trajmovc.ini_maxvel;
            // if (fabs(progress - tar_in_cartpos_arc) < 1e-6)
            // {
            //     MOTAL_INFO(" the last one jposition is : %f %f %f %f %f %f, discrete_cartpos = %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0],
            //            jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2], jpos_for_fit[discrete_num].jpos[3],
            //            jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5], discrete_cartpos.tran.x, discrete_cartpos.tran.y,
            //            discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
            // }
        }
        // MOTAL_INFO("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y,
        // discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // MOTAL_INFO(" **************************************** %dth %f
        // %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);

        if (shorter_than_last_blend_r)
        {
            if (progress >= TOPP.last_tc_blend_r)
            {
                TOPP.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (progress != 0.0)
            {
                for (int i = 0; i < stat.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[TOPP.lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }

            // MOTAL_INFO("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1],
            // jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos_arc - progress) <= TOPP.cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            TOPP.cur_blend_len = discrete_num;
            for (i = 0; i < stat.robot_joint_num; i++) { stat_robotproxy.cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(TOPP.cur_blend_len != discrete_num){
        //     MOTAL_INFO("*********************************************************** TOPP.cur_blend_len = %d, cur_tc_blend_jointpos = %f %f %f %f %f
        //     %f\n", TOPP.cur_blend_len,
        //             cur_tc_blend_jointpos[0], cur_tc_blend_jointpos[1], cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4],
        //             cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && TOPP.cur_tc_blend_r > SMALL){
        //     MOTAL_INFO("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1],
        //     jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((TOPP.cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < stat.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                TOPP.cur_tc_blend_r = 0;
                TOPP.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tar_in_cartpos_arc)
        {
            if (TOPP.cur_tc_blend_r <= SMALL)
            {
                TOPP.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tar_in_cartpos_arc)
        {
            progress = tar_in_cartpos_arc;
        }
    }
    /* update stat_robotproxy.cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        stat_robotproxy.cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        stat_robotproxy.cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        stat_robotproxy.cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        stat_robotproxy.cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        stat_robotproxy.cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        stat_robotproxy.cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (TOPP.cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            stat_robotproxy.cur_tc_blend_jointpos[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
            stat_robotproxy.cur_tc_blend_jointpos_sec[i] = stat_robotproxy.cur_tc_goal_jointpos[i];
        }
    }

    return 0;
}

//motionflag 获取movc圆心  get_movc_center
PmCartesian TOPP_member::get_movc_center(PmCartesian A, PmCartesian B, PmCartesian C)
{
    PmCartesian u, v;    //求出的新坐标系X,Y方向上的向量
    PmCartesian center;  //圆心
    PmCartesian AB, AC;  // AB: 向量AB， AC：向量AC
    PmCartesian ex, ey;  //ex: 新坐标系X轴单位向量 ，ey: 新坐标系Y轴单位向量
    PmCartesian vy, vx;  // C点做垂先和AB相交，所得的X轴上的向量。
    double xB, xC, yC;   // AB的长度。

    PmCartesian tempEx, tempEy;

    pmCartCartSub(&B, &A, &u);     //计算 u = B - A;
    pmCartCartSub(&C, &A, &v);     //计算 v = C - A;
    pmCartMag(&u, &xB);            //计算 xB = norm(u)
    pmCartUnit(&u, &ex);           //计算 ex = u/xB
    pmCartCartDot(&v, &ex, &xC);   //vx = （v * ex）*ex;
    pmCartScalMult(&ex, xC, &vx);  //计算 vy = v - vx;
    pmCartCartSub(&v, &vx, &vy);   //计算Y轴单位向量 ey = ey/|ey|
    pmCartUnit(&vy, &ey);          //计算 yC = |vy|
    pmCartMag(&vy, &yC);
    pmCartScalMult(&ex, 0.5 * xB, &tempEx);
    pmCartCartAdd(&A, &tempEx, &tempEx);
    pmCartScalMult(
        &ey, (-1 * xB * (xB - xC) * xC + xB * yC * yC) / (2 * xB * yC), &tempEy);  //计算center = A + 1/2 xB*ex + (-xB (xB - xC) xC + xB yC^2)/(2 xB yC)*ey
    pmCartCartAdd(&tempEx, &tempEy, &center);

    return center;
}

//motionflag 获取movc相关规划参数  toppuse
int TOPP_member::get_movc_planning_params(MovcStruct* circle, ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end)
{
    //the same as pmMovcInit()
    //计算圆心
    PmCartesian startPosition;
    PmCartesian midPosition;
    PmCartesian endPosition;

    PmCartesian center;
    PmCartesian uCenterStart;
    PmCartesian uCenterMid;
    PmCartesian uCenterEnd;
    PmCartesian normal, normal1, normal2;

    double radius;
    double theta, scm, mce, sce;
    double thetaMD;
    double startEndDot;
    double startMidDot;
    double midEndDot;
    PmRotationMatrix rotStart;
    PmRotationMatrix rotEnd;
    PmRotationMatrix rot;
    PmRotationMatrix startInv;
    PmRpy rpyStart, rpyEnd;
    PmRotationVector abc;
    double normalMag;
    double cosTheta;
    int turn;
    int use_sm = 1;
    startPosition = start->tran;
    midPosition = mid->tran;
    endPosition = end->tran;

    // MOTAL_INFO(" --------pmMovcInit------- : startPosition =%f   %f  %f  \n  ",startPosition.x,startPosition.y,startPosition.z);
    // MOTAL_INFO(" --------pmMovcInit------- : midPosition =%f   %f  %f \n ",midPosition.x,midPosition.y,midPosition.z);
    // MOTAL_INFO(" --------pmMovcInit------- : end =%f   %f  %f  \n ",endPosition.x,endPosition.y,endPosition.z);

    center = get_movc_center(startPosition, midPosition, endPosition);
    // MOTAL_INFO(" --------pmMovcInit------- : center =%f   %f  %f  \n  ",center.x,center.y,center.z);

    pmCartCartSub(&startPosition, &center, &uCenterStart);
    pmCartCartSub(&midPosition, &center, &uCenterMid);
    pmCartCartSub(&endPosition, &center, &uCenterEnd);

    // MOTAL_INFO("uCenterStart = %f %f %f \n ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    // MOTAL_INFO("uCenterMid = %f %f %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    // MOTAL_INFO("uCenterMid = %f %f %f \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    //计算半径
    pmCartMag(&uCenterStart, &radius);
    // MOTAL_INFO(" --------pmMovcInit------- : radius =%f   \n  ",radius);

    //计算旋转方向
    //计算圆心角
    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    cosTheta = startEndDot / radius / radius;
    if (cosTheta > 1.0)
        cosTheta = 1.0;
    else if (cosTheta < -1.0)
        cosTheta = -1.0;
    theta = acos(cosTheta);

    // MOTAL_INFO(" --------pmMovcInit------- : theta =%f   \n  ",theta);

    //计算姿态变化，
    if (fabs(theta) < TP_MIN_ARC_ANGLE)
    {
        return TP_ERR_FAIL;
    }
    //计算法向量
    pmCartCartDot(&uCenterStart, &uCenterMid, &startMidDot);
    scm = acos(startMidDot / radius / radius);

    pmCartCartDot(&uCenterMid, &uCenterEnd, &midEndDot);
    mce = acos(midEndDot / radius / radius);

    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    sce = acos(startEndDot / radius / radius);

    // MOTAL_INFO(" --------pmMovcInit------- : scm =%f   mce =%f \n  ",scm,mce);

    // MOTAL_INFO(" --------pmMovcInit------- : uCenterStart =%f   %f  %f  \n  ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    // MOTAL_INFO(" --------pmMovcInit------- : uCenterMid =%f   %f  %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    // MOTAL_INFO(" --------pmMovcInit------- : uCenterEnd =%f   %f  %f  \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    // if(fabs(theta - PM_PI)>TP_MIN_ARC_ANGLE){
    // 	pmCartCartCross(&uCenterStart, &uCenterEnd, &normal);
    // }
    /*
	if(fabs(scm - PM_PI)>TP_MIN_ARC_ANGLE){
		pmCartCartCross(&uCenterStart, &uCenterMid, &normal);
	}
	else if(fabs(mce - PM_PI)>TP_MIN_ARC_ANGLE){
		pmCartCartCross(&uCenterMid, &uCenterEnd, &normal);
		use_sm = 0;
	}
	else{
		return TP_ERR_FAIL;
	}
	*/
    if (fabs(mce - PM_PI) > TP_MIN_ARC_ANGLE)
    {
        pmCartCartCross(&uCenterMid, &uCenterEnd, &normal);
    }
    else
    {
        pmCartCartCross(&uCenterEnd, &uCenterStart, &normal);
    }
    pmCartMag(&normal, &normalMag);

    if (normalMag < TP_MIN_ARC_ANGLE)
    {
        return TP_ERR_FAIL;
    }

    normal.x /= normalMag;
    normal.y /= normalMag;
    normal.z /= normalMag;

    // 计算旋转方向
    pmCartCartCross(&uCenterStart, &uCenterMid, &normal1);
    pmCartCartCross(&uCenterStart, &uCenterEnd, &normal2);
    pmCartUnitEq(&normal1);
    pmCartUnitEq(&normal2);
    // MOTAL_INFO("normal1 = %f %f %f \n ",normal1.x,normal1.y,normal1.z);
    // MOTAL_INFO("normal2 = %f %f %f \n ",normal2.x,normal2.y,normal2.z);
    if (fabs(normal1.x - normal2.x) < TP_MIN_ARC_ANGLE && fabs(normal1.y - normal2.y) < TP_MIN_ARC_ANGLE && fabs(normal1.z - normal2.z) < TP_MIN_ARC_ANGLE)
    {
        //
        if (scm > sce)
        {
            if (use_sm)
            {
                pmCartCartCross(&uCenterMid, &uCenterStart, &normal);
            }
            else
            {
                pmCartCartCross(&uCenterEnd, &uCenterMid, &normal);
            }
            theta = PM_2_PI - theta;
        }
    }
    else
    {
        theta = PM_2_PI - theta;
    }
    pmCartUnitEq(&normal);
    // MOTAL_INFO(" --------pmMovcInit------- : normal =%f   %f  %f  \n  ",normal.x,normal.y,normal.z);

    rpyStart.y = start->a * PM_PI / 180.0;
    rpyStart.p = start->b * PM_PI / 180.0;
    rpyStart.r = start->c * PM_PI / 180.0;

    rpyEnd.y = end->a * PM_PI / 180.0;
    rpyEnd.p = end->b * PM_PI / 180.0;
    rpyEnd.r = end->c * PM_PI / 180.0;

    pmRpyMatConvert(&rpyStart, &rotStart);
    pmRpyMatConvert(&rpyEnd, &rotEnd);

    pmMatInv(&rotStart, &startInv);
    // pmMatMatMult(&startInv, &rotEnd,&rot);
    pmMatMatMult(&rotEnd, &startInv, &rot);

    pmMatRotConvert(&rot, &abc);

    // MOTAL_INFO(" --------pmMovcInit------- : abc = %f   %f   %f    %f   \n  ",abc.s,abc.x,abc.y,abc.z);

    circle->abc = abc;
    circle->normal = normal;
    circle->radius = radius;
    circle->start.tran = startPosition;
    circle->start.rot = rotStart;
    circle->end.tran = endPosition;
    circle->end.rot = rotEnd;
    circle->center = center;
    //circle->turn = 1;
    circle->angle = theta;
    return 0;
}

//motionflag 获取movc插补位置 toppuse
int TOPP_member::get_movc_pos(MovcStruct* circle, const double portion, ZucPose* discrete_cartpos)
{
    //圆心角(弧度) * 当前弧长/总弧长
    PmRotationVector rotK;
    PmRotationMatrix rot;
    PmRotationMatrix rotEnd;
    PmRpy rpyEnd;
    rotK.s = circle->angle * portion;
    rotK.x = circle->normal.x;
    rotK.y = circle->normal.y;
    rotK.z = circle->normal.z;
    PmCartesian p1, p2, xyz;
    // get position
    pmCartCartSub(&(circle->start.tran), &(circle->center), &p1);
    pmRotMatConvert(&rotK, &rot);
    pmMatCartMult(&rot, &p1, &p2);
    pmCartCartAdd(&p2, &(circle->center), &xyz);
    discrete_cartpos->tran = xyz;
    // get gesture
    PmRotationMatrix RotStart, RotEnd;                                  //起始点、结束点
    memcpy(&RotStart, &(circle->start.rot), sizeof(PmRotationMatrix));  //起始点旋转矩阵
    memcpy(&RotEnd, &(circle->end.rot), sizeof(PmRotationMatrix));      //结束点旋转矩阵
    quaternion_interp(&RotStart, &RotEnd, portion, discrete_cartpos);
    return 0;
}

//motionflag 四原数插补  toppuse
int TOPP_member::quaternion_interp(PmRotationMatrix* const Rot_0, PmRotationMatrix* const Rot_1, const double t, ZucPose* discrete_cartpos)
{
    double cos_angle = 0.0;
    double q_0[4] = {0};
    double q_1[4] = {0};
    double q_0_s[4] = {0};  // inv of q_0
    double q_1_s[4] = {0};  // inv of q_1
    double B[4] = {0};
    double Q_t1[4] = {0};
    double Q_t2[4] = {0};
    double Q_t3[4] = {0};
    double Q_1[4] = {0};
    double Q_2[4] = {0};
    double Q_3[4] = {0};
    double temp1[4] = {0};
    double temp2[4] = {0};
    double q_t[4] = {0};
    rot2quat(Rot_0, q_0);
    rot2quat(Rot_1, q_1);
    cos_angle = q_0[0] * q_1[0] + q_0[1] * q_1[1] + q_0[2] * q_1[2] + q_0[3] * q_1[3];
    if (cos_angle < 0)
    {
        for (int i = 0; i < 4; i++) { q_1[i] = -q_1[i]; }
    }
    quartic_basic(t, B);
    get_quat_inv(q_0, q_0_s);
    get_quat_inv(q_1, q_1_s);

    quat_quat_mult(q_0_s, q_0, Q_1);
    quat_quat_mult(q_0_s, q_1, Q_2);
    quat_quat_mult(q_1_s, q_1, Q_3);

    power_quat(Q_1, B[1], Q_t1);
    power_quat(Q_2, B[2], Q_t2);
    power_quat(Q_3, B[3], Q_t3);

    quat_quat_mult(q_0, Q_t1, temp1);
    quat_quat_mult(temp1, Q_t2, temp2);
    quat_quat_mult(temp2, Q_t3, q_t);
    PmRpy Rpy_t;
    quat_rpy_convert(q_t, &Rpy_t);
    discrete_cartpos->a = Rpy_t.y * 180.0 / PM_PI;
    discrete_cartpos->b = Rpy_t.p * 180.0 / PM_PI;
    discrete_cartpos->c = Rpy_t.r * 180.0 / PM_PI;

    return 0;
}

//motionflag 旋转矩阵转四原数
void TOPP_member::rot2quat(PmRotationMatrix* const m, double* q)
{
    double RotM[9] = {0};
    int flag = 0;
    double max_square = 0.0;
    double max_ref = 0.0;
    double mult = 0.0;
    double w = 0.0;
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double w_s = 0.0;
    double x_s = 0.0;
    double y_s = 0.0;
    double z_s = 0.0;

    RotM[0] = m->x.x;
    RotM[1] = m->y.x;
    RotM[2] = m->z.x;
    RotM[3] = m->x.y;
    RotM[4] = m->y.y;
    RotM[5] = m->z.y;
    RotM[6] = m->x.z;
    RotM[7] = m->y.z;
    RotM[8] = m->z.z;

    w_s = RotM[0] + RotM[4] + RotM[8];
    x_s = RotM[0] - RotM[4] - RotM[8];
    y_s = RotM[4] - RotM[0] - RotM[8];
    z_s = RotM[8] - RotM[0] - RotM[4];
    max_square = w_s;
    if (x_s > max_square)
    {
        max_square = x_s;
        flag = 1;
    }

    if (y_s > max_square)
    {
        max_square = y_s;
        flag = 2;
    }

    if (z_s > max_square)
    {
        max_square = z_s;
        flag = 3;
    }
    max_ref = 0.5 * sqrt(max_square + 1);
    mult = 0.25 / max_ref;
    switch (flag)
    {
    case 0:
        w = max_ref;
        x = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[6] - RotM[2]);
        z = mult * (RotM[1] - RotM[3]);
        break;

    case 1:
        x = max_ref;
        w = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[6] + RotM[2]);
        break;

    case 2:
        y = max_ref;
        w = mult * (RotM[6] - RotM[2]);
        x = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[5] + RotM[7]);
        break;

    case 3:
        z = max_ref;
        w = mult * (RotM[1] - RotM[3]);
        x = mult * (RotM[6] + RotM[2]);
        y = mult * (RotM[5] + RotM[7]);
        break;
    }

    if (w > 0 || w == 0)
    {
        q[0] = w;
        q[1] = x;
        q[2] = y;
        q[3] = z;
    }
    else
    {
        q[0] = -w;
        q[1] = -x;
        q[2] = -y;
        q[3] = -z;
    }
}

//motionflag 获取直线运动的位置  topp
int TOPP_member::get_line_position(ZucPose* start, double* dist_unit_vector, const double len, ZucPose* line_position)
{
    line_position->tran.x = start->tran.x + dist_unit_vector[0] * len;
    line_position->tran.y = start->tran.y + dist_unit_vector[1] * len;
    line_position->tran.z = start->tran.z + dist_unit_vector[2] * len;
    return 0;
}

int TOPP_member::get_line_gesture(ZucPose* start, ZucPose* end, const double len, ZucPose* gesture)
{
    int i;
    double start_r, start_p, start_y, end_r, end_p, end_y;
    double start_quat[4] = {0};
    double end_quat[4] = {0};
    double start_quat_inv[4] = {0};
    double end_quat_inv[4] = {0};
    double delta_quat[4] = {0};
    double cos_angle = 0.0;
    double B[4] = {0};
    double Q_t1[4] = {0};
    double Q_t2[4] = {0};
    double Q_t3[4] = {0};
    double Q_1[4] = {0};
    double Q_2[4] = {0};
    double Q_3[4] = {0};
    double temp1[4] = {0};
    double temp2[4] = {0};
    double q_t[4] = {0};
    PmRotationMatrix* Rot_t;
    PmRpy Rpy_t;

    start_y = start->a * PM_PI / 180.0;  // x_axis
    start_p = start->b * PM_PI / 180.0;  // y_axis
    start_r = start->c * PM_PI / 180.0;  // z_axis

    end_y = end->a * PM_PI / 180.0;  // x_axis
    end_p = end->b * PM_PI / 180.0;
    end_r = end->c * PM_PI / 180.0;

    rpy_quat_convert(start_r, start_p, start_y, start_quat);
    rpy_quat_convert(end_r, end_p, end_y, end_quat);
    // MOTAL_INFO("start_quat = %f %f %f %f, end_quat = %f %f %f %f\n", start_quat[0], start_quat[1], start_quat[2], start_quat[3], end_quat[0], end_quat[1], end_quat[2], end_quat[3]);

    cos_angle = start_quat[0] * end_quat[0] + start_quat[1] * end_quat[1] + start_quat[2] * end_quat[2] + start_quat[3] * end_quat[3];
    if (cos_angle < 0)
    {
        for (i = 0; i < 4; i++) { end_quat[i] = -end_quat[i]; }
    }
    /*四次多项式插补*/
    // quartic_basic(len, B);
    // get_quat_inv(start_quat, start_quat_inv);
    // get_quat_inv(end_quat, end_quat_inv);
    // quat_quat_mult(start_quat_inv, start_quat, Q_1);
    // quat_quat_mult(start_quat_inv, end_quat, Q_2);
    // quat_quat_mult(end_quat_inv, end_quat, Q_3);
    // power_quat(Q_1, B[1], Q_t1);
    // power_quat(Q_2, B[2], Q_t2);
    // power_quat(Q_3, B[3], Q_t3);
    // quat_quat_mult(start_quat, Q_t1, temp1);
    // quat_quat_mult(temp1, Q_t2, temp2);
    // quat_quat_mult(temp2, Q_t3, q_t);

    /*slerp*/
    get_quat_inv(start_quat, start_quat_inv);
    quat_quat_mult(end_quat, start_quat_inv, delta_quat);
    double delta_Q_t[4] = {0};
    power_quat(delta_quat, len, delta_Q_t);
    quat_quat_mult(delta_Q_t, start_quat, q_t);

    quat_rpy_convert(q_t, &Rpy_t);
    gesture->a = Rpy_t.y * 180.0 / PM_PI;
    gesture->b = Rpy_t.p * 180.0 / PM_PI;
    gesture->c = Rpy_t.r * 180.0 / PM_PI;
    return 0;
}

int TOPP_member::rpy_quat_convert(double r, double p, double y, double* quat)
{
    PmRotationMatrix m;
    double sa, sb, sg;
    double ca, cb, cg;

    // sa = sin(r);
    // sb = sin(p);
    // sg = sin(y);

    // ca = cos(r);
    // cb = cos(p);
    // cg = cos(y);

    if (fabs(fabs(r) - PM_PI) <= 1e-6)
    {
        sa = 0.0;
        ca = -1.0;
    }
    else if (fabs(r) <= 1e-6)
    {
        sa = 0.0;
        ca = 1.0;
    }
    else
    {
        sa = sin(r);
        ca = cos(r);
    }

    if (fabs(fabs(p) - PM_PI) <= 1e-6)
    {
        sb = 0.0;
        cb = -1.0;
    }
    else if (fabs(p) <= 1e-6)
    {
        sb = 0.0;
        cb = 1.0;
    }
    else
    {
        sb = sin(p);
        cb = cos(p);
    }

    if (fabs(fabs(y) - PM_PI) <= 1e-6)
    {
        sg = 0.0;
        cg = -1.0;
    }
    else if (fabs(y) <= 1e-6)
    {
        sg = 0.0;
        cg = 1.0;
    }
    else
    {
        sg = sin(y);
        cg = cos(y);
    }

    m.x.x = ca * cb;
    m.y.x = ca * sb * sg - sa * cg;
    m.z.x = ca * sb * cg + sa * sg;

    m.x.y = sa * cb;
    m.y.y = sa * sb * sg + ca * cg;
    m.z.y = sa * sb * cg - ca * sg;

    m.x.z = -sb;
    m.y.z = cb * sg;
    m.z.z = cb * cg;

    double RotM[9] = {0};
    int flag = 0;
    double max_square = 0.0;
    double max_ref = 0.0;
    double mult = 0.0;
    double w = 0.0;
    double x = 0.0;
    y = 0.0;
    double z = 0.0;
    double w_s = 0.0;
    double x_s = 0.0;
    double y_s = 0.0;
    double z_s = 0.0;

    RotM[0] = m.x.x;
    RotM[1] = m.y.x;
    RotM[2] = m.z.x;
    RotM[3] = m.x.y;
    RotM[4] = m.y.y;
    RotM[5] = m.z.y;
    RotM[6] = m.x.z;
    RotM[7] = m.y.z;
    RotM[8] = m.z.z;

    w_s = RotM[0] + RotM[4] + RotM[8];
    x_s = RotM[0] - RotM[4] - RotM[8];
    y_s = RotM[4] - RotM[0] - RotM[8];
    z_s = RotM[8] - RotM[0] - RotM[4];
    max_square = w_s;

    if (x_s > max_square)
    {
        max_square = x_s;
        flag = 1;
    }

    if (y_s > max_square)
    {
        max_square = y_s;
        flag = 2;
    }

    if (z_s > max_square)
    {
        max_square = z_s;
        flag = 3;
    }

    max_ref = 0.5 * sqrt(max_square + 1);
    mult = 0.25 / max_ref;

    switch (flag)
    {
    case 0:
        w = max_ref;
        x = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[6] - RotM[2]);
        z = mult * (RotM[1] - RotM[3]);
        break;

    case 1:
        x = max_ref;
        w = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[6] + RotM[2]);
        break;

    case 2:
        y = max_ref;
        w = mult * (RotM[6] - RotM[2]);
        x = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[5] + RotM[7]);
        break;

    case 3:
        z = max_ref;
        w = mult * (RotM[1] - RotM[3]);
        x = mult * (RotM[6] + RotM[2]);
        y = mult * (RotM[5] + RotM[7]);
        break;
    }

    if (w > 0 || w == 0)
    {
        quat[0] = w;
        quat[1] = x;
        quat[2] = y;
        quat[3] = z;
    }
    else
    {
        quat[0] = -w;
        quat[1] = -x;
        quat[2] = -y;
        quat[3] = -z;
    }
    // quat[0] = 0.5 * pmSqrt(1.0 + m.x.x + m.y.y + m.z.z);
    // if (fabs(quat[0]) > CLOSE_TO_ZERO) {
    //     quat[1] = (m.y.z - m.z.y) / 4 / quat[0];
    //     quat[2] = (m.z.x - m.x.z) / 4 / quat[0];
    //     quat[3] = (m.x.y - m.y.x) / 4 / quat[0];
    // } else {
    //     quat[0] = 0;
    //     quat[1] = pmSqrt(1.0 + m.x.x - m.y.y - m.z.z) / 2.0;
    //     quat[2] = pmSqrt(1.0 + m.y.y - m.x.x - m.z.z) / 2.0;
    //     quat[3] = pmSqrt(1.0 + m.z.z - m.y.y - m.x.x) / 2.0;

    //     if (quat[1] > quat[2] && quat[1] > quat[3]) {
    //         if (m.x.y < 0.0) {
    //             quat[2] *= -1;
    //         }
    //         if (m.x.z < 0.0) {
    //             quat[3] *= -1;
    //         }
    //     } else if (quat[2] > quat[3]) {
    //         if (m.x.y < 0.0) {
    //             quat[1] *= -1;
    //         }
    //         if (m.y.z < 0.0) {
    //             quat[3] *= -1;
    //         }
    //     } else {
    //         if (m.x.z < 0.0) {
    //             quat[1] *= -1;
    //         }
    //         if (m.y.z < 0.0) {
    //             quat[2] *= -1;
    //         }
    //     }
    // }
    return 0;
}

//motionflag 四原数插补算系数 topp
void TOPP_member::quartic_basic(const double t, double* B)
{
    double b0 = 0.0;
    double b1 = 0.0;
    double b2 = 0.0;
    double b3 = 0.0;
    double B0 = 0.0;
    double B1 = 0.0;
    double B2 = 0.0;
    double B3 = 0.0;

    b0 = -0.5 * t + 1.5 * pow(t, 2) - 1.5 * pow(t, 3) + 0.5 * pow(t, 4);
    b1 = 1 - 3 * pow(t, 2) + 2.5 * pow(t, 3) - 0.5 * pow(t, 4);
    b2 = 0.5 * t + 1.5 * pow(t, 2) - 0.5 * pow(t, 3) - 0.5 * pow(t, 4);
    b3 = -0.5 * pow(t, 3) + 0.5 * pow(t, 4);

    B0 = b0 + b1 + b2 + b3;
    B1 = b1 + b2 + b3;
    B2 = b2 + b3;
    B3 = b3;

    B[0] = B0;
    B[1] = B1;
    B[2] = B2;
    B[3] = B3;
}

//motionflag 获取四原数求逆  toppuse
int TOPP_member::get_quat_inv(double* quat, double* quat_inv)
{
    quat_inv[0] = quat[0];
    quat_inv[1] = -quat[1];
    quat_inv[2] = -quat[2];
    quat_inv[3] = -quat[3];

    return 0;
}

//motionflag 获取四原数求逆  toppuse
int TOPP_member::quat_quat_mult(double* quat1, double* quat2, double* quat_out)
{
    quat_out[0] = quat1[0] * quat2[0] - quat1[1] * quat2[1] - quat1[2] * quat2[2] - quat1[3] * quat2[3];
    if (quat_out[0] >= 0.0)
    {
        quat_out[1] = quat1[0] * quat2[1] + quat1[1] * quat2[0] + quat1[2] * quat2[3] - quat1[3] * quat2[2];
        quat_out[2] = quat1[0] * quat2[2] - quat1[1] * quat2[3] + quat1[2] * quat2[0] + quat1[3] * quat2[1];
        quat_out[3] = quat1[0] * quat2[3] + quat1[1] * quat2[2] - quat1[2] * quat2[1] + quat1[3] * quat2[0];
    }
    else
    {
        quat_out[0] = -quat_out[0];
        quat_out[1] = -(quat1[0] * quat2[1] + quat1[1] * quat2[0] + quat1[2] * quat2[3] - quat1[3] * quat2[2]);
        quat_out[2] = -(quat1[0] * quat2[2] - quat1[1] * quat2[3] + quat1[2] * quat2[0] + quat1[3] * quat2[1]);
        quat_out[3] = -(quat1[0] * quat2[3] + quat1[1] * quat2[2] - quat1[2] * quat2[1] + quat1[3] * quat2[0]);
    }
    return 0;
}

//motionflag 四原数求指数 topp
void TOPP_member::power_quat(const double* q, const double t, double* Q)
{
    double Q_log[4] = {0};
    log_quat(q, Q_log);
    for (int i = 0; i < 4; i++) { Q_log[i] = Q_log[i] * t; }
    exp_quat(Q_log, Q);
}

//motionflag 四原数求log topp
void TOPP_member::log_quat(const double* q, double* Q)
{
    double theta = 0.0;
    double s_theta = 0.0;
    if (q[0] > (1 - 1e-8))
    {
        Q[0] = 0.0;
        Q[1] = 0.0;
        Q[2] = 0.0;
        Q[3] = 0.0;
    }
    else
    {
        theta = acos(q[0]);
        s_theta = theta / sin(theta);
        Q[0] = 0.0;
        Q[1] = s_theta * q[1];
        Q[2] = s_theta * q[2];
        Q[3] = s_theta * q[3];
    }
}

//motionflag 四原数求exp topp
void TOPP_member::exp_quat(const double* q, double* Q)
{
    double q_n = 0.0;
    double s_theta = 0.0;
    double c_theta = 0.0;
    double temp = 0.0;
    norm_quat(q, &q_n);

    if (q_n < 1e-8)
    {
        Q[0] = 1.0;
        Q[1] = 0.0;
        Q[2] = 0.0;
        Q[3] = 0.0;
    }
    else
    {
        temp = 1 / q_n;
        s_theta = sin(q_n);
        c_theta = cos(q_n);
        Q[0] = c_theta;
        Q[1] = s_theta * temp * q[1];
        Q[2] = s_theta * temp * q[2];
        Q[3] = s_theta * temp * q[3];
    }
    //cout << q[0] << "\t";  cout << q[1] << "\t"; cout << q[2] << "\t"; cout << q[3] << endl;
    //cout << q_n << endl;
}

//motionflag 四原数求单位化 topp
void TOPP_member::norm_quat(const double* q, double* q_n) { *q_n = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); }

//motionflag 四原数rpy转换 topp
int TOPP_member::quat_rpy_convert(double* quat, PmRpy* const rpy)
{
    PmRotationMatrix m;
    double s = 0.0;
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    s = quat[0];
    x = quat[1];
    y = quat[2];
    z = quat[3];
    m.x.x = 1 - 2 * (y * y + z * z);
    m.y.x = 2 * (x * y + s * z);
    m.z.x = 2 * (x * z - s * y);
    m.x.y = 2 * (x * y - s * z);
    m.y.y = 1 - 2 * (x * x + z * z);
    m.z.y = 2 * (y * z + s * x);
    m.x.z = 2 * (x * z + s * y);
    m.y.z = 2 * (y * z - s * x);
    m.z.z = 1 - 2 * (x * x + y * y);

    double sy;
    sy = pmSqrt((m.x.x * m.x.x) + (m.x.y * m.x.y));
    rpy->p = atan2(-m.x.z, sy);

    if (fabs(rpy->p - (PM_PI / 2.0)) < RPY_P_FUZZ)
    {
        rpy->r = 0.0; /* force it */
        rpy->p = PM_PI / 2.0;
        rpy->y = atan2(m.y.x, m.y.y);
    }
    else if (fabs(rpy->p + (PM_PI / 2.0)) < RPY_P_FUZZ)
    {
        rpy->r = -0.0;
        rpy->p = -PM_PI / 2.0; /* force it */
        rpy->y = -atan2(m.y.x, m.y.y);
    }
    else
    {
        rpy->r = atan2(m.x.y / cos(rpy->p), m.x.x / cos(rpy->p));
        rpy->y = atan2(m.y.z / cos(rpy->p), m.z.z / cos(rpy->p));
    }
    return 0;
}

//motionflag 清队列 没用到
int TOPP_member::ClearBlendQueue()
{
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.front = 0;
        blend_seg_jpos.rear = 0;
        MOTAL_INFO("succeed init blend_seg_jpos space\n");
    }
    else
    {
        MOTAL_INFO("[ClearBlendQueue]  --->  blend_seg_jpos == NULL\n");
        return -1;
    }
    return 0;
}

//motionflag topp 判断队列是否满
int TOPP_member::BlendEnqueue(BlendJpos element_added)
{
    if ((blend_seg_jpos.rear + 1) % MAX_BLEND_LEN == blend_seg_jpos.front)
    {
        MOTAL_INFO("队列已满！\n");
        return -1;
    }
    blend_seg_jpos.jpos[blend_seg_jpos.rear] = element_added;
    blend_seg_jpos.rear = (blend_seg_jpos.rear + 1) % MAX_BLEND_LEN;
    return 0;
}

//motionflag toppuse 获取队列中的第一个点
int TOPP_member::GetBlendFront(BlendJpos* element_front)
{
    if (blend_seg_jpos.rear == blend_seg_jpos.front)
    {
        MOTAL_INFO("[GetBlendFront] Empty Queue!\n");
        return -1;
    }
    *element_front = blend_seg_jpos.jpos[blend_seg_jpos.front];
    blend_seg_jpos.front = (blend_seg_jpos.front + 1) % MAX_BLEND_LEN;
    return 0;
}
// //motionflag toppuse 计算topp相关系数
// int TOPP_member::get_pathvalue_coefs_pycubic(const int discrete_num, const ZucJPose* jpos_for_fit)
// {
// #if EN_TOPPRA
//     int i, j, k;
//     double delta_jpos = 0.0, pathdata = 0.0;
//     // double c[4][discrete_num - 1];
//     PyObject* jpos = PyList_New(discrete_num);
//     PyObject* path_data = PyList_New(discrete_num);
//     PyObject* list_c0 = PyList_New(discrete_num - 1);
//     PyObject* list_c1 = PyList_New(discrete_num - 1);
//     PyObject* list_c2 = PyList_New(discrete_num - 1);
//     PyObject* list_c3 = PyList_New(discrete_num - 1);

//     PyList_SetItem(path_data, 0, Py_BuildValue("d", pathdata));
//     // MOTAL_INFO("pathdata(0) = %f\n", pathdata);

//     TOPP.path_info[0].ss = pathdata;
//     TOPP.path_info[0].ss_seg_start = pathdata;
//     double joint_delta = 0.0;
//     for (i = 1; i < discrete_num; i++)
//     {
//         PyList_SetItem(list_c0, (i - 1), Py_BuildValue("d", 0.0));
//         PyList_SetItem(list_c1, (i - 1), Py_BuildValue("d", 0.0));
//         PyList_SetItem(list_c2, (i - 1), Py_BuildValue("d", 0.0));
//         PyList_SetItem(list_c3, (i - 1), Py_BuildValue("d", 0.0));
//         // if(motion_type == TC_JOINT){
//         //     delta_jpos = fabs(jpos_for_fit[i].jpos[zucmotCommand.joint] - jpos_for_fit[i - 1].jpos[zucmotCommand.joint]);
//         // } else {
//         //     /* Cartesian space motion  */
//         delta_jpos = 0.0;
//         int max_num = 0;
//         for (j = 0; j < stat.robot_joint_num; j++)
//         {
//             joint_delta = fabs(jpos_for_fit[i].jpos[j] - jpos_for_fit[i - 1].jpos[j]);
//             if (delta_jpos < joint_delta)
//             {
//                 delta_jpos = joint_delta;
//                 max_num = j;
//             }
//         }
//         if (fabs(delta_jpos - 0.0) < 1e-6)
//         {
//             MOTAL_INFO("jpos_for_fit[%d] = %f %f %f %f %f %f, jpos_for_fit[i-1] = %f %f %f %f %f %f\n",
//                         i,
//                         jpos_for_fit[i].jpos[0],
//                         jpos_for_fit[i].jpos[1],
//                         jpos_for_fit[i].jpos[2],
//                         jpos_for_fit[i].jpos[3],
//                         jpos_for_fit[i].jpos[4],
//                         jpos_for_fit[i].jpos[5],
//                         jpos_for_fit[i - 1].jpos[0],
//                         jpos_for_fit[i - 1].jpos[1],
//                         jpos_for_fit[i - 1].jpos[2],
//                         jpos_for_fit[i - 1].jpos[3],
//                         jpos_for_fit[i - 1].jpos[4],
//                         jpos_for_fit[i - 1].jpos[5]);
//         }
//         // }
//         double delta_gp = SS_INCRE * delta_jpos / TOPP.disjlen;
//         pathdata = pathdata + delta_gp;
//         // pathdata = pathdata + SS_INCRE;
//         TOPP.path_info[i].ss = pathdata;
//         TOPP.path_info[i].ss_seg_start = pathdata;

//         PyList_SetItem(path_data, i, Py_BuildValue("d", pathdata));
//         if ((TOPP.cur_tc_blend_r > SMALL) && (i == (TOPP.cur_blend_len - REFIT_NUM)))
//         {
//             TOPP.last_final_pathp = pathdata;
//             // MOTAL_INFO("TOPP.last_final_pathp = %f\n", TOPP.last_final_pathp);
//         }
//         // MOTAL_INFO("path_data = %f, delta_jpos = %f, max_num = %d, TOPP.disjlen = %f\n", pathdata, delta_jpos, max_num, TOPP.disjlen);
//     }
//     TOPP.path_info[discrete_num - 1].ss_seg_start = TOPP.path_info[discrete_num - 2].ss;
//     // MOTAL_INFO(" *********************** cur_blend_len = %d, discrete_num = %d, cur_last_blend_len = %d, final_ss = %f *********************** \n", TOPP.cur_blend_len, discrete_num, TOPP.cur_last_blend_len, pathdata);

//     PyObject* coefs_a;
//     PyObject* coefs_b;
//     PyObject* coefs_c;
//     PyObject* coefs_d;
//     for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
//     {
//         for (i = 0; i < discrete_num; i++) { PyList_SetItem(jpos, i, Py_BuildValue("d", jpos_for_fit[i].jpos[jnum])); }
//         if (callPyCubic(path_data, jpos, list_c0, list_c1, list_c2, list_c3, discrete_num) < 0)
//         {
//             MOTAL_INFO("Error in Python code, exception was printed, jnum = %d.\n", jnum);
//         }
//         else
//         {
//             for (k = 0; k < discrete_num - 1; k++)
//             {
//                 coefs_a = PyList_GetItem(list_c0, k);
//                 coefs_b = PyList_GetItem(list_c1, k);
//                 coefs_c = PyList_GetItem(list_c2, k);
//                 coefs_d = PyList_GetItem(list_c3, k);
//                 TOPP.path_info[k].pathq_coefs[jnum][0] = PyFloat_AsDouble(coefs_a);
//                 TOPP.path_info[k].pathq_coefs[jnum][1] = PyFloat_AsDouble(coefs_b);
//                 TOPP.path_info[k].pathq_coefs[jnum][2] = PyFloat_AsDouble(coefs_c);
//                 TOPP.path_info[k].pathq_coefs[jnum][3] = PyFloat_AsDouble(coefs_d);
//                 // if(k == TOPP.cur_blend_len){
//                 //     MOTAL_INFO("TOPP.path_info[k].ss = %f\n", TOPP.path_info[k].ss);
//                 // }
//             }
//             k = discrete_num - 1;
//             TOPP.path_info[k].pathq_coefs[jnum][0] = TOPP.path_info[k - 1].pathq_coefs[jnum][0];
//             TOPP.path_info[k].pathq_coefs[jnum][1] = TOPP.path_info[k - 1].pathq_coefs[jnum][1];
//             TOPP.path_info[k].pathq_coefs[jnum][2] = TOPP.path_info[k - 1].pathq_coefs[jnum][2];
//             TOPP.path_info[k].pathq_coefs[jnum][3] = TOPP.path_info[k - 1].pathq_coefs[jnum][3];
//         }
//     }

//     // for (i = 0; i < discrete_num; i++)
//     // {
//     //     MOTAL_INFO("++++%dth, ss = %f, ss_seg_start = %f, coefs:\n", i, TOPP.path_info[i].ss, TOPP.path_info[i].ss_seg_start);
//     //     MOTAL_INFO("****%dth coefs:\n", i);
//     //     MOTAL_INFO("coefs[0]: %f %f %f %f %f %f\n",
//     //                 TOPP.path_info[i].pathq_coefs[0][0],
//     //                 TOPP.path_info[i].pathq_coefs[1][0],
//     //                 TOPP.path_info[i].pathq_coefs[2][0],
//     //                 TOPP.path_info[i].pathq_coefs[3][0],
//     //                 TOPP.path_info[i].pathq_coefs[4][0],
//     //                 TOPP.path_info[i].pathq_coefs[5][0]);
//     //     MOTAL_INFO("coefs[1]: %f %f %f %f %f %f\n",
//     //                 TOPP.path_info[i].pathq_coefs[0][1],
//     //                 TOPP.path_info[i].pathq_coefs[1][1],
//     //                 TOPP.path_info[i].pathq_coefs[2][1],
//     //                 TOPP.path_info[i].pathq_coefs[3][1],
//     //                 TOPP.path_info[i].pathq_coefs[4][1],
//     //                 TOPP.path_info[i].pathq_coefs[5][1]);
//     //     MOTAL_INFO("coefs[2]: %f %f %f %f %f %f\n",
//     //                 TOPP.path_info[i].pathq_coefs[0][2],
//     //                 TOPP.path_info[i].pathq_coefs[1][2],
//     //                 TOPP.path_info[i].pathq_coefs[2][2],
//     //                 TOPP.path_info[i].pathq_coefs[3][2],
//     //                 TOPP.path_info[i].pathq_coefs[4][2],
//     //                 TOPP.path_info[i].pathq_coefs[5][2]);
//     //     MOTAL_INFO("coefs[3]: %f %f %f %f %f %f\n",
//     //                 TOPP.path_info[i].pathq_coefs[0][3],
//     //                 TOPP.path_info[i].pathq_coefs[1][3],
//     //                 TOPP.path_info[i].pathq_coefs[2][3],
//     //                 TOPP.path_info[i].pathq_coefs[3][3],
//     //                 TOPP.path_info[i].pathq_coefs[4][3],
//     //                 TOPP.path_info[i].pathq_coefs[5][3]);
//     // }

//     if (TOPP.cur_tc_blend_r <= SMALL)
//     {
//         TOPP.last_final_pathp = 0.0;
//     }

//     Py_DECREF(jpos);
//     Py_DECREF(path_data);
//     Py_DECREF(list_c0);
//     Py_DECREF(list_c1);
//     Py_DECREF(list_c2);
//     Py_DECREF(list_c3);
// #endif
//     return 0;
// }

//motionflag toppuse 计算topp相关系数
int TOPP_member::sget_blend_pathvalue_coefs_csaps(const int discrete_num, const double start_path_data, const ZucJPose* jpos_for_fit)
{
#if EN_TOPPRA
    /***************** use python csaps *****************/
    int i = 0, j = 0, k = 0;
    double smooth_para = 0.9, pathdata = start_path_data;

    double cur_tc_refit_jpos[2 * REFIT_NUM][MAX_AXIS_INT] = {0};
    int index = 0;
    for (int j = 1; j <= 2 * REFIT_NUM; j++)
    {
        index = TOPP.cur_last_blend_len + j;
        for (int jnum = 0; jnum < stat.robot_joint_num; jnum++) { cur_tc_refit_jpos[j - 1][jnum] = TOPP.path_info[index].pathq_coefs[jnum][3]; }
        // MOTAL_INFO("%dth cur_tc_refit_jpos = %f %f %f %f %f %f\n",
        //             j,
        //             cur_tc_refit_jpos[j - 1][0],
        //             cur_tc_refit_jpos[j - 1][1],
        //             cur_tc_refit_jpos[j - 1][2],
        //             cur_tc_refit_jpos[j - 1][3],
        //             cur_tc_refit_jpos[j - 1][4],
        //             cur_tc_refit_jpos[j - 1][5]);
    }

    PyObject* jpos = PyList_New(discrete_num + 3 * REFIT_NUM);
    PyObject* path_data = PyList_New(discrete_num + 3 * REFIT_NUM);
    PyObject* w = PyList_New(discrete_num + 3 * REFIT_NUM);  //weights
    PyObject* list_c0 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c1 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c2 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c3 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyList_SetItem(path_data, 0, Py_BuildValue("d", pathdata));
    PyList_SetItem(w, 0, Py_BuildValue("d", MAX_NUM));
    double delta_jpos = 0.0;
    double joint_delta = 0.0;
    for (i = 1; i < discrete_num + 3 * REFIT_NUM; i++)
    {
        PyList_SetItem(list_c0, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c1, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c2, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c3, (i - 1), Py_BuildValue("d", 0.0));
        delta_jpos = 0.0;
        if (i < REFIT_NUM)
        {
            // MOTAL_INFO("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                joint_delta = fabs(last_tc_refit_jpos[i][jnum] - last_tc_refit_jpos[i - 1][jnum]);
                // MOTAL_INFO(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // MOTAL_INFO("\n");
        }
        else if (i == REFIT_NUM)
        {
            // MOTAL_INFO("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                joint_delta = fabs(jpos_for_fit[i - REFIT_NUM].jpos[jnum] - last_tc_refit_jpos[i - 1][jnum]);
                // MOTAL_INFO(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // MOTAL_INFO("\n");
        }
        else if (i > REFIT_NUM && i < discrete_num + REFIT_NUM)
        {
            // MOTAL_INFO("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                joint_delta = fabs(jpos_for_fit[i - REFIT_NUM].jpos[jnum] - jpos_for_fit[i - REFIT_NUM - 1].jpos[jnum]);
                // MOTAL_INFO(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // MOTAL_INFO("\n");
        }
        else if (i == discrete_num + REFIT_NUM)
        {
            // MOTAL_INFO("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                joint_delta = fabs(cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM)][jnum] - jpos_for_fit[i - REFIT_NUM - 1].jpos[jnum]);
                // MOTAL_INFO(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // MOTAL_INFO("\n");
        }
        else if (i > discrete_num + REFIT_NUM)
        {
            // MOTAL_INFO("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                joint_delta = fabs(cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM)][jnum] - cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM) - 1][jnum]);
                // MOTAL_INFO(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // MOTAL_INFO("\n");
        }

        pathdata = pathdata + SS_INCRE * delta_jpos / TOPP.disjlen;
        // MOTAL_INFO("pathdata = %f, delta_jpos = %f\n", pathdata, delta_jpos);
        // pathdata = pathdata + SS_INCRE;
        PyList_SetItem(path_data, i, Py_BuildValue("d", pathdata));
        PyList_SetItem(w, i, Py_BuildValue("d", CLOSE_TO_ZERO));
        if (i >= (REFIT_NUM - 2) && i < (discrete_num + 2 * REFIT_NUM))
        {
            // if (i == REFIT_NUM - 2)
            // {
            //     MOTAL_INFO("start_path_data = %f, pathdata = %f\n", start_path_data, pathdata);
            // }
            TOPP.blend_path_info[i - (REFIT_NUM - 2)].ss = pathdata;
            TOPP.blend_path_info[i - (REFIT_NUM - 2)].ss_seg_start = pathdata;
        }
    }
    i = (discrete_num + 2 * REFIT_NUM) - 1 - (REFIT_NUM - 2);
    // MOTAL_INFO("[before] ---> TOPP.blend_path_info[last_point].ss_seg_start = %f\n", TOPP.blend_path_info[i].ss_seg_start);
    TOPP.blend_path_info[i].ss_seg_start = TOPP.blend_path_info[i - 1].ss;
    // MOTAL_INFO("[after] ---> TOPP.blend_path_info[last_point].ss_seg_start = %f\n", TOPP.blend_path_info[i].ss_seg_start);
    for (i = 0; i <= REFIT_NUM; i++) { PyList_SetItem(w, i, Py_BuildValue("d", MAX_NUM)); }
    for (i = 0; i <= 2 * REFIT_NUM; i++) { PyList_SetItem(w, (discrete_num + 3 * REFIT_NUM - 1) - i, Py_BuildValue("d", MAX_NUM)); }

    PyObject* coefs_a;
    PyObject* coefs_b;
    PyObject* coefs_c;
    PyObject* coefs_d;

    for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
    {
        for (i = 0; i < REFIT_NUM; i++)
        {
            PyList_SetItem(jpos, i, Py_BuildValue("d", last_tc_refit_jpos[i][jnum]));
            // MOTAL_INFO("jpos = %f\n", last_tc_refit_jpos[i][jnum]);
        }
        // MOTAL_INFO("*******\n");
        for (i = 0; i < discrete_num; i++)
        {
            PyList_SetItem(jpos, (i + REFIT_NUM), Py_BuildValue("d", jpos_for_fit[i].jpos[jnum]));
            // MOTAL_INFO("jpos = %f\n", jpos_for_fit[i].jpos[jnum]);
        }
        // MOTAL_INFO("*******\n");
        for (i = 0; i < 2 * REFIT_NUM; i++)
        {
            PyList_SetItem(jpos, (i + discrete_num + REFIT_NUM), Py_BuildValue("d", cur_tc_refit_jpos[i][jnum]));
            // MOTAL_INFO("jpos = %f\n", cur_tc_refit_jpos[i][jnum]);
        }
        if (callPyCsapsF(path_data, jpos, w, smooth_para, list_c0, list_c1, list_c2, list_c3, discrete_num + 3 * REFIT_NUM) < 0)
        {
            MOTAL_INFO("Error in Python code, exception was printed, blend jnum = %d.\n", jnum);
        }
        else
        {
            for (k = REFIT_NUM - 2; k < (discrete_num + 2 * REFIT_NUM) - 1; k++)
            {
                coefs_a = PyList_GetItem(list_c0, k);
                coefs_b = PyList_GetItem(list_c1, k);
                coefs_c = PyList_GetItem(list_c2, k);
                coefs_d = PyList_GetItem(list_c3, k);
                TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][0] = PyFloat_AsDouble(coefs_a);
                TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][1] = PyFloat_AsDouble(coefs_b);
                TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][2] = PyFloat_AsDouble(coefs_c);
                TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][3] = PyFloat_AsDouble(coefs_d);
            }
            k = (discrete_num + 2 * REFIT_NUM) - 1;
            j = k - 1;
            TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][0] = TOPP.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][0];
            TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][1] = TOPP.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][1];
            TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][2] = TOPP.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][2];
            TOPP.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][3] = TOPP.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][3];
        }
    }

    // for (i = 0; i < (discrete_num + REFIT_NUM) + 2; i++)
    // {
    //     MOTAL_INFO("++++%dth, ss = %f, ss_seg_start = %f, coefs:\n", i, TOPP.blend_path_info[i].ss, TOPP.blend_path_info[i].ss_seg_start);
    //     MOTAL_INFO("coefs[0]: %f %f %f %f %f %f\n",
    //                 TOPP.blend_path_info[i].pathq_coefs[0][0],
    //                 TOPP.blend_path_info[i].pathq_coefs[1][0],
    //                 TOPP.blend_path_info[i].pathq_coefs[2][0],
    //                 TOPP.blend_path_info[i].pathq_coefs[3][0],
    //                 TOPP.blend_path_info[i].pathq_coefs[4][0],
    //                 TOPP.blend_path_info[i].pathq_coefs[5][0]);
    //     MOTAL_INFO("coefs[1]: %f %f %f %f %f %f\n",
    //                 TOPP.blend_path_info[i].pathq_coefs[0][1],
    //                 TOPP.blend_path_info[i].pathq_coefs[1][1],
    //                 TOPP.blend_path_info[i].pathq_coefs[2][1],
    //                 TOPP.blend_path_info[i].pathq_coefs[3][1],
    //                 TOPP.blend_path_info[i].pathq_coefs[4][1],
    //                 TOPP.blend_path_info[i].pathq_coefs[5][1]);
    //     MOTAL_INFO("coefs[2]: %f %f %f %f %f %f\n",
    //                 TOPP.blend_path_info[i].pathq_coefs[0][2],
    //                 TOPP.blend_path_info[i].pathq_coefs[1][2],
    //                 TOPP.blend_path_info[i].pathq_coefs[2][2],
    //                 TOPP.blend_path_info[i].pathq_coefs[3][2],
    //                 TOPP.blend_path_info[i].pathq_coefs[4][2],
    //                 TOPP.blend_path_info[i].pathq_coefs[5][2]);
    //     MOTAL_INFO("coefs[3]: %f %f %f %f %f %f\n",
    //                 TOPP.blend_path_info[i].pathq_coefs[0][3],
    //                 TOPP.blend_path_info[i].pathq_coefs[1][3],
    //                 TOPP.blend_path_info[i].pathq_coefs[2][3],
    //                 TOPP.blend_path_info[i].pathq_coefs[3][3],
    //                 TOPP.blend_path_info[i].pathq_coefs[4][3],
    //                 TOPP.blend_path_info[i].pathq_coefs[5][3]);
    // }
    Py_DECREF(jpos);
    Py_DECREF(path_data);
    Py_DECREF(w);
    Py_DECREF(list_c0);
    Py_DECREF(list_c1);
    Py_DECREF(list_c2);
    Py_DECREF(list_c3);
#endif
    return 0;
}

//motionflag 设置topp参数至规划
int RobotProxy::set_constraints_toppra(ConstraintsToppra& constraints)
{
    int i;
    if (stat.motion.motion_type == TC_JOINT)
    {
        // constraints.req_vel[1] = trajjointmov.vel;
        double jvel = stat_robotproxy.trajjointmov.vel;
        double jacc = stat_robotproxy.trajjointmov.acc;
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            constraints.vel_lim[i][0] = -jvel;
            constraints.vel_lim[i][1] = jvel;
            constraints.acc_lim[i][0] = -jacc;
            constraints.acc_lim[i][1] = jacc;
        }
    }
    else
    {
        // constraints.req_vel[1] = stat_robotproxy.trajlinearmov.vel;
        for (i = 0; i < stat.robot_joint_num; i++)
        {
            constraints.vel_lim[i][0] = -stat.config.JointConfig[i].MaxVel;
            constraints.vel_lim[i][1] = stat.config.JointConfig[i].MaxVel;
            constraints.acc_lim[i][0] = -stat.config.JointConfig[i].MaxAccel;
            constraints.acc_lim[i][1] = stat.config.JointConfig[i].MaxAccel;  //traj_planning中的宏“ACC_BOUND”的值已更改为joint[0].acc_limit
        }
    }
    double fv_value = 0.0;
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        // MOTAL_INFO("stat_robotproxy.tor_limit = %f, vf_params = %f, MaxVel = %f ", stat_robotproxy.tor_limit[i], vf_params[i], stat.config.JointConfig[i].MaxVel);
        fv_value = conf_robotproxy.vf_params[i] * stat.config.JointConfig[i].MaxVel * PM_PI / 180.0;
        constraints.tor_lim[i][1] = stat_robotproxy.tor_limit[i] * TOPP.usertor_portion;  // - fv_value;
        if (constraints.tor_lim[i][1] < 2 * fv_value)
        {
            constraints.tor_lim[i][1] = 2 * fv_value;
        }
        constraints.tor_lim[i][0] = -constraints.tor_lim[i][1];
        // MOTAL_INFO("final tor_lim = %f %f\n", constraints.tor_lim[i][1], constraints.tor_lim[i][0]);
    }

    constraints.lim_add[0] = 1;  //joint vel constraint
    constraints.lim_add[2] = 1;  //不能只添加关节速度约束
    constraints.lim_add[1] = TOPP.userset_filter_size;
    return 0;
}

//motionflag 设置topp参数至规划
int TOPP_member::get_blend_path_information(const int blend_jpos_len, ConstraintsToppra& constraints)  //计算最优化参数: vel, acc, tor
{
    int i, j, k;
    double delta_s_value;  // 用于代入到表达式中计算q(s)一阶导
    PathCoefs pathcoefs;
    for (i = 0; i < blend_jpos_len; i++)
    {
        delta_s_value = blend_path_info[i].ss - blend_path_info[i].ss_seg_start;
        for (j = 0; j < robot_joint_num; j++)
        {
            for (k = 0; k < 4; k++) { pathcoefs.pathq_coefs[j][k] = blend_path_info[i].pathq_coefs[j][k]; }
        }
        blend_joint_vel_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // if (constraints.lim_add[1] == 1)
        // {
        //     blend_joint_acc_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // }
        if (constraints.lim_add[2] == 1)
        {
            blend_joint_tor_constraints_params(i, delta_s_value, pathcoefs, constraints);
        }
    }
    blend_interpolate_discretization(blend_jpos_len);

    return 0;
}

//motionflag 设置topp参数至规划
int TOPP_member::blend_joint_vel_constraints_params(const int i_th,
                                                    const double delta_s_value,
                                                    PathCoefs& pathcoefs,
                                                    ConstraintsToppra& constraints)  // get xbound
{
    // 根据关节速度约束计算路径速度的边界
    int i, j;
    double sd_min, sd_max;
    double q_d_s[MAX_AXIS_INT];  //q对s的一阶导
    sd_min = 0;
    sd_max = MAX_NUM;
    for (i = 0; i < robot_joint_num; i++)
    {
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        if (q_d_s[i] > 0)
        {
            sd_max = get_min((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_min);
        }
        if (q_d_s[i] < 0)
        {
            sd_max = get_min((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_min);
        }
    }
    blend_path_info[i_th].xbound[0] = sd_min * sd_min;
    blend_path_info[i_th].xbound[1] = sd_max * sd_max;
    return 0;
}

//motionflag 设置topp参数至规划  啥也没干
int TOPP_member::blend_joint_acc_constraints_params(const int i_th,
                                                    const double delta_s_value,
                                                    PathCoefs& pathcoefs,
                                                    ConstraintsToppra& constraints)  // get jacc_params: jacc_a, jacc_b
{
    // int i;
    // // double q_d_s[MAX_AXIS_INT], q_dd_s[MAX_AXIS_INT];//q对s的一阶导和二阶导
    // for (i = 0; i < stat.robot_joint_num; i++)
    // {
    //     TOPP.blend_path_info[i_th].jacc_a[i] =
    //         3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
    //     TOPP.blend_path_info[i_th].jacc_b[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    // }
    return 0;
}

//motionflag 设置topp参数至规划
int RobotProxy::blend_joint_tor_constraints_params(const int i_th,
                                                   const double delta_s_value,
                                                   PathCoefs& pathcoefs,
                                                   ConstraintsToppra& constraints)  // jtor_params: jtor_a, jtor_b, jtor_c, jtor_d
{
    int i;
    double q_s[MAX_AXIS_INT], q_d_s[MAX_AXIS_INT], q_dd_s[MAX_AXIS_INT];
    double q_equal[MAX_AXIS_INT], qd_equal[MAX_AXIS_INT], qdd_equal[MAX_AXIS_INT], tor_equal[MAX_AXIS_INT], tg[MAX_AXIS_INT], tf[MAX_AXIS_INT],
        tfv[MAX_AXIS_INT], tfc[MAX_AXIS_INT], f_offset[MAX_AXIS_INT], zero_array[MAX_AXIS_INT];
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_s[i] = pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) * delta_s_value + pathcoefs.pathq_coefs[i][1] * pmSq(delta_s_value) +
                 pathcoefs.pathq_coefs[i][2] * delta_s_value + pathcoefs.pathq_coefs[i][3];
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        q_dd_s[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    }

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = 0;
        zero_array[i] = 0;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);

    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.blend_path_info[i_th].jtor_c[i] = tor_equal[i]; }

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = q_d_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.blend_path_info[i_th].jtor_a[i] = tor_equal[i] - TOPP.blend_path_info[i_th].jtor_c[i]; }

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = q_d_s[i] * M_PI / 180;
        qdd_equal[i] = q_dd_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.blend_path_info[i_th].jtor_b[i] = tor_equal[i] - TOPP.blend_path_info[i_th].jtor_c[i] - tfv[i] - tfc[i]; }

    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.blend_path_info[i_th].jtor_c[i] += tfc[i]; }
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        TOPP.blend_path_info[i_th].jtor_d[i] =
            conf_robotproxy.vf_params[i] * q_d_s[i] * M_PI / 180;  //在计算可达集的时候使用，不过忽略粘滞摩擦力的话影响应该不算很大
    }
    return 0;
}

//motionflag 设置topp参数至规划
int RobotProxy::blend_interpolate_discretization(const int blend_jpos_len)
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();

    int i = 0, j = 0, k = 0;
    double delta = 0.0, ss_next = 0.0, delta_tar = 0.0;
    for (i = 0; i < 1; i++)
    {
        TOPP.blend_path_info[i].min_time = 0.0;
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // TOPP.blend_path_info[i].jacc_a[j] = TOPP.blend_path_info[i].jacc_a[k];
            // TOPP.blend_path_info[i].jacc_b[j] = TOPP.blend_path_info[i].jacc_b[k];

            TOPP.blend_path_info[i].jtor_a[j] = TOPP.blend_path_info[i].jtor_a[k];
            TOPP.blend_path_info[i].jtor_b[j] = TOPP.blend_path_info[i].jtor_b[k];
            TOPP.blend_path_info[i].jtor_c[j] = TOPP.blend_path_info[i].jtor_c[k];
            TOPP.blend_path_info[i].jtor_d[j] = TOPP.blend_path_info[i].jtor_d[k];
        }
    }
    //这里计算min_time的方式需要修正：因为在规划中是把转接段归于前一运动段的，这样的话，在计算min_time的时候就应该使用前一运动段的运动类型and最大速度
    double jpos1[MAX_AXIS_INT] = {0}, jpos2[MAX_AXIS_INT] = {0};
    ZucPose cartpos1, cartpos2;
    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    double delta_jpos = 0.0;
    double delta_tar_cart = 0.0;
    double min_time_cart = 0.0;

    for (i = 1; i < blend_jpos_len; i++)
    {
        ss_next = TOPP.blend_path_info[i - 1].ss;
        delta = TOPP.blend_path_info[i].ss - ss_next;

        for (j = 0; j < stat.robot_joint_num; j++)
        {
            jpos1[j] = TOPP.blend_path_info[i - 1].pathq_coefs[j][3] + TOPP.blend_path_info[i - 1].pathq_coefs[j][2] * delta +
                       TOPP.blend_path_info[i - 1].pathq_coefs[j][1] * delta * delta + TOPP.blend_path_info[i - 1].pathq_coefs[j][0] * delta * delta * delta;
            jpos2[j] = TOPP.blend_path_info[i].pathq_coefs[j][3] + TOPP.blend_path_info[i].pathq_coefs[j][2] * delta +
                       TOPP.blend_path_info[i].pathq_coefs[j][1] * delta * delta + TOPP.blend_path_info[i].pathq_coefs[j][0] * delta * delta * delta;
        }
        kine->fkine(RobJointVal(jpos1), cartpos1, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        kine->fkine(RobJointVal(jpos2), cartpos2, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        // kinematicsForward(&(zucmotConfig.tool_offset), jpos1, &cartpos1, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        // kinematicsForward(&(zucmotConfig.tool_offset), jpos2, &cartpos2, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        delta_tar_cart = pmSqrt(pmSq(cartpos1.tran.x - cartpos2.tran.x) + pmSq(cartpos1.tran.y - cartpos2.tran.y) + pmSq(cartpos1.tran.z - cartpos2.tran.z));
        if (stat.motion.motion_type == TC_JOINT)
        {
            delta_tar = 0.0;
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                delta_jpos = fabs(jpos1[jnum] - jpos2[jnum]);
                if (delta_tar < delta_jpos)
                {
                    delta_tar = delta_jpos;
                }
            }

            // double j1 = 0.0, j2 = 0.0, j3 = 0.0, j4 = 0.0;
            // j1 = TOPP.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][3] + TOPP.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         TOPP.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][1] * delta * delta + TOPP.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;
            // j2 = TOPP.blend_path_info[i].pathq_coefs[zucmotCommand.joint][3] + TOPP.blend_path_info[i].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         TOPP.blend_path_info[i].pathq_coefs[zucmotCommand.joint][1] * delta * delta + TOPP.blend_path_info[i].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;

            // j3 = TOPP.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][3] + TOPP.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][2] * delta +
            //         TOPP.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][1] * delta * delta + TOPP.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;
            // j4 = TOPP.blend_path_info[i].pathq_coefs[lasttc_jointNum][3] + TOPP.blend_path_info[i].pathq_coefs[lasttc_jointNum][2] * delta +
            //         TOPP.blend_path_info[i].pathq_coefs[lasttc_jointNum][1] * delta * delta + TOPP.blend_path_info[i].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;

            // if(fabs(j3 - j4) > fabs(j1 - j2)){
            //     delta_tar = fabs(j3 - j4);
            // } else {
            //     delta_tar = fabs(j1 - j2);
            // }

            // if (trajjointmov.ini_maxvel > V_FUZZ)
            // {  //min_time的计算需改进，主要是转接和调倍率的影响
            TOPP.blend_path_info[i].min_time = delta_tar / stat_robotproxy.trajjointmov.ini_maxvel;
            // }
            // else
            // {
            //     TOPP.blend_path_info[i].min_time = 1e8;
            // }
            // if (tcp_vel_limit_toppra < V_FUZZ)
            // {
            //     TOPP.blend_path_info[i].min_time = 1e8;
            // }
            // else
            // {
            min_time_cart = delta_tar_cart / TOPP.tcp_vel_limit_toppra;
            if (min_time_cart > TOPP.blend_path_info[i].min_time)
            {
                TOPP.blend_path_info[i].min_time = min_time_cart;
            }
            // }
            // MOTAL_INFO("tcp_vel_limit_toppra = %f, min_time = %f, min_time_cart = %f, delta_tar_cart = %f\n", tcp_vel_limit_toppra, TOPP.path_info[i].min_time, min_time_cart, delta_tar_cart);
        }
        else
        {
            delta_tar = delta_tar_cart;
            // double cart_vel = 0.0;
            if (stat.motion.motion_type == TC_MOVC)
            {
                // cart_vel = trajmovc.ini_maxvel;
                // if (cart_vel > tcp_vel_limit_toppra)
                // {
                //     cart_vel = tcp_vel_limit_toppra;
                // }
                TOPP.blend_path_info[i].min_time = delta_tar / stat_robotproxy.trajmovc.ini_maxvel;
                // if (cart_vel > V_FUZZ)
                // {
                //     TOPP.blend_path_info[i].min_time = delta_tar / cart_vel;
                // }
                // else
                // {
                //     TOPP.blend_path_info[i].min_time = 1e8;
                // }
            }
            else if (stat.motion.motion_type == TC_LINEAR)
            {
                if (stat_robotproxy.trajlinearmov.using_abc_vel)
                {
                    min_time_cart =
                        delta_tar / stat_robotproxy.trajlinearmov.ini_maxvel;  //以姿态做规划主轴时，这里暂时使用轴的最大速度，后续需要改善的话再进行修改
                    if (min_time_cart > TOPP.blend_path_info[i].min_time)
                    {
                        TOPP.blend_path_info[i].min_time = min_time_cart;
                    }
                }
                else
                {
                    // cart_vel = stat_robotproxy.trajlinearmov.ini_maxvel;
                    // if (cart_vel > tcp_vel_limit_toppra)
                    // {
                    //     cart_vel = tcp_vel_limit_toppra;
                    // }
                    // if (cart_vel > V_FUZZ)
                    // {
                    TOPP.blend_path_info[i].min_time = delta_tar / stat_robotproxy.trajlinearmov.ini_maxvel;
                    // }
                    // else
                    // {
                    //     TOPP.blend_path_info[i].min_time = 1e8;
                    // }
                }
            }
            // MOTAL_INFO("[blend interpolate discretization] : cartpos2 = %f %f %f %f %f %f\n", cartpos2.tran.x, cartpos2.tran.y, cartpos2.tran.z, cartpos2.a, cartpos2.b, cartpos2.c);
        }

        // MOTAL_INFO("[blend_interpolate]: min_time = %f\n", TOPP.blend_path_info[i].min_time);
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // TOPP.blend_path_info[i].jacc_a[j] =
            //     TOPP.blend_path_info[i - 1].jacc_a[k] - 2 * delta * TOPP.blend_path_info[i - 1].jacc_b[k];
            // TOPP.blend_path_info[i].jacc_b[j] = TOPP.blend_path_info[i - 1].jacc_b[k];

            TOPP.blend_path_info[i].jtor_a[j] = TOPP.blend_path_info[i - 1].jtor_a[k] - 2 * delta * TOPP.blend_path_info[i - 1].jtor_b[k];
            TOPP.blend_path_info[i].jtor_b[j] = TOPP.blend_path_info[i - 1].jtor_b[k];
            TOPP.blend_path_info[i].jtor_c[j] = TOPP.blend_path_info[i - 1].jtor_c[k];
            TOPP.blend_path_info[i].jtor_d[j] = TOPP.blend_path_info[i - 1].jtor_d[k];
        }
    }
    return 0;
}

//motionflag 设置topp参数至规划
int TOPP_member::get_path_information(const int jpos_len, ConstraintsToppra& constraints)  //计算最优化参数: vel, acc, tor
{
    int i, j, k;
    double delta_s_value;  // 用于代入到表达式中计算q(s)一阶导
    PathCoefs pathcoefs;
    for (i = 0; i < jpos_len; i++)
    {
        delta_s_value = path_info[i].ss - path_info[i].ss_seg_start;
        for (j = 0; j < robot_joint_num; j++)
        {
            for (k = 0; k < 4; k++) { pathcoefs.pathq_coefs[j][k] = path_info[i].pathq_coefs[j][k]; }
        }
        joint_vel_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // if (constraints.lim_add[1] == 1)
        // {
        //     joint_acc_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // }
        if (constraints.lim_add[2] == 1)
        {
            joint_tor_constraints_params(i, delta_s_value, pathcoefs, constraints);
        }
    }
    interpolate_discretization(jpos_len);

    return 0;
}

//motionflag topp 关节速度限制参数
int TOPP_member::joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints)  // get xbound
{
    // 根据关节速度约束计算路径速度的边界
    int i, j;
    double sd_min, sd_max;
    double q_d_s[MAX_AXIS_INT];  //q对s的一阶导
    sd_min = 0;
    sd_max = MAX_NUM;
    for (i = 0; i < robot_joint_num; i++)
    {
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        if (q_d_s[i] > 0)
        {
            sd_max = get_min((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_min);
        }
        if (q_d_s[i] < 0)
        {
            sd_max = get_min((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_min);
        }
    }
    path_info[i_th].xbound[0] = sd_min * sd_min;
    path_info[i_th].xbound[1] = sd_max * sd_max;
    return 0;
}

//motionflag topp 关节加速度限制参数
int TOPP_member::joint_acc_constraints_params(const int i_th,
                                              const double delta_s_value,
                                              PathCoefs& pathcoefs,
                                              ConstraintsToppra& constraints)  // get jacc_params: jacc_a, jacc_b
{
    // int i;
    // // double q_d_s[MAX_AXIS_INT], q_dd_s[MAX_AXIS_INT];//q对s的一阶导和二阶导
    // for (i = 0; i < robot_joint_num; i++)
    // {
    //     TOPP.path_info[i_th].jacc_a[i] =
    //         3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
    //     TOPP.path_info[i_th].jacc_b[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    // }
    return 0;
}

//motionflag topp 关节力矩限制参数
int RobotProxy::joint_tor_constraints_params(const int i_th,
                                             const double delta_s_value,
                                             PathCoefs& pathcoefs,
                                             ConstraintsToppra& constraints)  // jtor_params: jtor_a, jtor_b, jtor_c, jtor_d
{
    int i;
    double q_s[MAX_AXIS_INT], q_d_s[MAX_AXIS_INT], q_dd_s[MAX_AXIS_INT];
    double q_equal[MAX_AXIS_INT], qd_equal[MAX_AXIS_INT], qdd_equal[MAX_AXIS_INT], tor_equal[MAX_AXIS_INT], tg[MAX_AXIS_INT], tf[MAX_AXIS_INT],
        tfv[MAX_AXIS_INT], tfc[MAX_AXIS_INT], f_offset[MAX_AXIS_INT], zero_array[MAX_AXIS_INT];
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_s[i] = pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) * delta_s_value + pathcoefs.pathq_coefs[i][1] * pmSq(delta_s_value) +
                 pathcoefs.pathq_coefs[i][2] * delta_s_value + pathcoefs.pathq_coefs[i][3];
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        q_dd_s[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    }

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = 0;
        zero_array[i] = 0;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    // MOTAL_INFO("q_equal = %f %f %f %f %f %f, tor_equal = %f %f %f %f %f %f\n", q_equal[0], q_equal[1], q_equal[2], q_equal[3], q_equal[4], q_equal[5], tor_equal[0], tor_equal[1], tor_equal[2],
    //         tor_equal[3], tor_equal[4], tor_equal[5]);

    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.path_info[i_th].jtor_c[i] = tor_equal[i]; }

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = q_d_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.path_info[i_th].jtor_a[i] = tor_equal[i] - TOPP.path_info[i_th].jtor_c[i]; }
    // MOTAL_INFO("jtor_a = %f %f %f %f %f %f\n", TOPP.path_info[i_th].jtor_a[0], TOPP.path_info[i_th].jtor_a[1], TOPP.path_info[i_th].jtor_a[2],
    //         TOPP.path_info[i_th].jtor_a[3], TOPP.path_info[i_th].jtor_a[4], TOPP.path_info[i_th].jtor_a[5]);

    for (i = 0; i < stat.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = q_d_s[i] * M_PI / 180;
        qdd_equal[i] = q_dd_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < stat.robot_joint_num; i++) { TOPP.path_info[i_th].jtor_b[i] = tor_equal[i] - TOPP.path_info[i_th].jtor_c[i] - tfv[i] - tfc[i]; }
    // MOTAL_INFO("jtor_b = %f %f %f %f %f %f\n", TOPP.path_info[i_th].jtor_b[0], TOPP.path_info[i_th].jtor_b[1], TOPP.path_info[i_th].jtor_b[2],
    //         TOPP.path_info[i_th].jtor_b[3], TOPP.path_info[i_th].jtor_b[4], TOPP.path_info[i_th].jtor_b[5]);
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        TOPP.path_info[i_th].jtor_c[i] += tfc[i];
        TOPP.path_info[i_th].jtor_c[i] *= TOPP.usertor_portion;
    }
    // MOTAL_INFO("jtor_c = %f %f %f %f %f %f\n", TOPP.path_info[i_th].jtor_c[0], TOPP.path_info[i_th].jtor_c[1], TOPP.path_info[i_th].jtor_c[2],
    //         TOPP.path_info[i_th].jtor_c[3], TOPP.path_info[i_th].jtor_c[4], TOPP.path_info[i_th].jtor_c[5]);
    for (i = 0; i < stat.robot_joint_num; i++)
    {
        TOPP.path_info[i_th].jtor_d[i] = conf_robotproxy.vf_params[i] * q_d_s[i] * M_PI / 180;  //在计算可达集的时候使用，不过忽略粘滞摩擦力的话影响应该不算很大
    }

    // MOTAL_INFO("jtor_d = %f %f %f %f %f %f\n", TOPP.path_info[i_th].jtor_d[0], TOPP.path_info[i_th].jtor_d[1], TOPP.path_info[i_th].jtor_d[2],
    //         TOPP.path_info[i_th].jtor_d[3], TOPP.path_info[i_th].jtor_d[4], TOPP.path_info[i_th].jtor_d[5]);
    // MOTAL_INFO("\n");
    return 0;
}

//motionflag topp 切点
int RobotProxy::interpolate_discretization(const int jpos_len)
{
    axisgroup::ToolData& tool_offset_refv = axisgroup::RobotToolMgr::get_rob_tool(stat.cur_tool_id);
    axisgroup::UsrFrame& base_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.base_frame_id);
    axisgroup::UsrFrame& user_offset_refv = axisgroup::UsrFrameMgr::get_usr_frame(stat.cur_usrframe_id);
    PmRpy base_offset;  //ybh need fix
    base_offset.y = base_offset_refv.pose().a;
    base_offset.p = base_offset_refv.pose().b;
    base_offset.r = base_offset_refv.pose().c;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(stat.rob_id)->kine();
    int i = 0, j = 0, k = 0;
    double delta = 0.0, ss_next = 0.0, delta_tar = 0.0;
    for (i = 0; i < 1; i++)
    {
        TOPP.path_info[i].min_time = 0.0;
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // TOPP.path_info[i].jacc_a[j] = TOPP.path_info[i].jacc_a[k];
            // TOPP.path_info[i].jacc_b[j] = TOPP.path_info[i].jacc_b[k];

            TOPP.path_info[i].jtor_a[j] = TOPP.path_info[i].jtor_a[k];
            TOPP.path_info[i].jtor_b[j] = TOPP.path_info[i].jtor_b[k];
            TOPP.path_info[i].jtor_c[j] = TOPP.path_info[i].jtor_c[k];
            TOPP.path_info[i].jtor_d[j] = TOPP.path_info[i].jtor_d[k];
            // MOTAL_INFO("[task]: %dth jtor_param = %f %f %f %f\n", j, TOPP.path_info[i].jtor_a[j], TOPP.path_info[i].jtor_b[j], TOPP.path_info[i].jtor_c[j],
            //         TOPP.path_info[i].jtor_d[j]);
        }
    }

    double jpos1[MAX_AXIS_INT] = {0}, jpos2[MAX_AXIS_INT] = {0};
    ZucPose cartpos1, cartpos2;
    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    double delta_jpos = 0.0;
    double min_time_cart = 0.0;
    for (i = 1; i < jpos_len; i++)
    {
        ss_next = TOPP.path_info[i - 1].ss;
        delta = TOPP.path_info[i].ss - ss_next;
        for (j = 0; j < stat.robot_joint_num; j++)
        {
            jpos1[j] = TOPP.path_info[i - 1].pathq_coefs[j][3] + TOPP.path_info[i - 1].pathq_coefs[j][2] * delta +
                       TOPP.path_info[i - 1].pathq_coefs[j][1] * delta * delta + TOPP.path_info[i - 1].pathq_coefs[j][0] * delta * delta * delta;
            jpos2[j] = TOPP.path_info[i].pathq_coefs[j][3] + TOPP.path_info[i].pathq_coefs[j][2] * delta + TOPP.path_info[i].pathq_coefs[j][1] * delta * delta +
                       TOPP.path_info[i].pathq_coefs[j][0] * delta * delta * delta;
        }
        kine->fkine(RobJointVal(jpos1), cartpos1, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        kine->fkine(RobJointVal(jpos2), cartpos2, &tool_offset_refv.pose(), &user_offset_refv.pose(), &base_offset);
        // kinematicsForward(&(zucmotConfig.tool_offset), jpos1, &cartpos1, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        // kinematicsForward(&(zucmotConfig.tool_offset), jpos2, &cartpos2, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        // MOTAL_INFO("jpos1 = %f, jpos2 = %f, cart1 = %f %f %f, cart2 = %f %f %f\n", jpos1[0], jpos2[0], cartpos1.tran.x, cartpos1.tran.y, cartpos1.tran.z, cartpos2.tran.x, cartpos2.tran.y, cartpos2.tran.z);
        double delta_tar_cart =
            pmSqrt(pmSq(cartpos1.tran.x - cartpos2.tran.x) + pmSq(cartpos1.tran.y - cartpos2.tran.y) + pmSq(cartpos1.tran.z - cartpos2.tran.z));

        if (stat.motion.motion_type == TC_JOINT)
        {
            // double j1 = 0.0, j2 = 0.0, j3 = 0.0, j4 = 0.0;
            // j1 = TOPP.path_info[i - 1].pathq_coefs[zucmotCommand.joint][3] + TOPP.path_info[i - 1].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         TOPP.path_info[i - 1].pathq_coefs[zucmotCommand.joint][1] * delta * delta + TOPP.path_info[i - 1].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;
            // j2 = TOPP.path_info[i].pathq_coefs[zucmotCommand.joint][3] + TOPP.path_info[i].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         TOPP.path_info[i].pathq_coefs[zucmotCommand.joint][1] * delta * delta + TOPP.path_info[i].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;

            // j3 = TOPP.path_info[i - 1].pathq_coefs[lasttc_jointNum][3] + TOPP.path_info[i - 1].pathq_coefs[lasttc_jointNum][2] * delta +
            //         TOPP.path_info[i - 1].pathq_coefs[lasttc_jointNum][1] * delta * delta + TOPP.path_info[i - 1].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;
            // j4 = TOPP.path_info[i].pathq_coefs[lasttc_jointNum][3] + TOPP.path_info[i].pathq_coefs[lasttc_jointNum][2] * delta +
            //         TOPP.path_info[i].pathq_coefs[lasttc_jointNum][1] * delta * delta + TOPP.path_info[i].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;

            // if(fabs(j3 - j4) > fabs(j1 - j2)){
            //     delta_tar = fabs(j3 - j4);
            // } else {
            //     delta_tar = fabs(j1 - j2);
            // }
            delta_tar = 0.0;
            for (int jnum = 0; jnum < stat.robot_joint_num; jnum++)
            {
                delta_jpos = fabs(jpos1[jnum] - jpos2[jnum]);
                if (delta_tar < delta_jpos)
                {
                    delta_tar = delta_jpos;
                }
            }

            // if (trajjointmov.ini_maxvel > V_FUZZ)
            // {  //min_time的计算需改进，主要是转接和调倍率的影响
            TOPP.path_info[i].min_time = delta_tar / stat_robotproxy.trajjointmov.ini_maxvel;
            // }
            // else
            // {
            //     TOPP.path_info[i].min_time = 1e8;
            // }
            // if (tcp_vel_limit_toppra < V_FUZZ)
            // {
            //     TOPP.path_info[i].min_time = 1e8;
            // }
            // else
            // {
            min_time_cart = delta_tar_cart / TOPP.tcp_vel_limit_toppra;
            if (min_time_cart > TOPP.path_info[i].min_time)
            {
                TOPP.path_info[i].min_time = min_time_cart;
            }
            // }
        }
        else
        {
            delta_tar = delta_tar_cart;
            // double cart_vel = 0.0;
            if (stat.motion.motion_type == TC_MOVC)
            {
                TOPP.path_info[i].min_time = delta_tar / stat_robotproxy.trajmovc.ini_maxvel;
                // if (cart_vel > tcp_vel_limit_toppra)
                // {
                //     cart_vel = tcp_vel_limit_toppra;
                // }
                // if (min_time_cart > TOPP.path_info[i].min_time)
                // {
                //     TOPP.path_info[i].min_time = min_time_cart;
                // }
                // else
                // {
                //     TOPP.path_info[i].min_time = 1e8;
                // }
            }
            else if (stat.motion.motion_type == TC_LINEAR)
            {
                if (stat_robotproxy.trajlinearmov.using_abc_vel)
                {
                    min_time_cart =
                        delta_tar / stat_robotproxy.trajlinearmov.ini_maxvel;  //以姿态做规划主轴时，这里暂时使用轴的最大速度，后续需要改善的话再进行修改
                    if (min_time_cart > TOPP.path_info[i].min_time)
                    {
                        TOPP.path_info[i].min_time = min_time_cart;
                    }
                }
                else
                {
                    TOPP.path_info[i].min_time = delta_tar / stat_robotproxy.trajlinearmov.ini_maxvel;
                    // if (cart_vel > tcp_vel_limit_toppra)
                    // {
                    //     cart_vel = tcp_vel_limit_toppra;
                    // }
                    // if (min_time_cart > TOPP.path_info[i].min_time)
                    // {
                    //     TOPP.path_info[i].min_time = min_time_cart;
                    // }
                    // else
                    // {
                    //     TOPP.path_info[i].min_time = 1e8;
                    // }
                }
            }
        }

        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // TOPP.path_info[i].jacc_a[j] = TOPP.path_info[i - 1].jacc_a[k] - 2 * delta * TOPP.path_info[i - 1].jacc_b[k];
            // TOPP.path_info[i].jacc_b[j] = TOPP.path_info[i - 1].jacc_b[k];

            TOPP.path_info[i].jtor_a[j] = TOPP.path_info[i - 1].jtor_a[k] - 2 * delta * TOPP.path_info[i - 1].jtor_b[k];
            TOPP.path_info[i].jtor_b[j] = TOPP.path_info[i - 1].jtor_b[k];
            TOPP.path_info[i].jtor_c[j] = TOPP.path_info[i - 1].jtor_c[k];
            TOPP.path_info[i].jtor_d[j] = TOPP.path_info[i - 1].jtor_d[k];
            // MOTAL_INFO("[task]: %d, %dth jtor_param = %f %f %f %f\n", i, j, TOPP.path_info[i].jtor_a[j], TOPP.path_info[i].jtor_b[j], TOPP.path_info[i].jtor_c[j],
            //         TOPP.path_info[i].jtor_d[j]);
        }
        // MOTAL_INFO("interpolate: \n");
        // MOTAL_INFO("jtor_a = %f %f %f %f %f %f, %f %f %f %f %f %f\n", TOPP.path_info[i].jtor_a[0], TOPP.path_info[i].jtor_a[1], TOPP.path_info[i].jtor_a[2],
        //         TOPP.path_info[i].jtor_a[3], TOPP.path_info[i].jtor_a[4], TOPP.path_info[i].jtor_a[5], TOPP.path_info[i].jtor_a[6],
        //         TOPP.path_info[i].jtor_a[7], TOPP.path_info[i].jtor_a[8],
        //         TOPP.path_info[i].jtor_a[9], TOPP.path_info[i].jtor_a[10], TOPP.path_info[i].jtor_a[11]);
        // MOTAL_INFO("jtor_b = %f %f %f %f %f %f, %f %f %f %f %f %f\n", TOPP.path_info[i].jtor_b[0], TOPP.path_info[i].jtor_b[1], TOPP.path_info[i].jtor_b[2],
        //         TOPP.path_info[i].jtor_b[3], TOPP.path_info[i].jtor_b[4], TOPP.path_info[i].jtor_b[5], TOPP.path_info[i].jtor_b[6],
        //         TOPP.path_info[i].jtor_b[7], TOPP.path_info[i].jtor_b[8],
        //         TOPP.path_info[i].jtor_b[9], TOPP.path_info[i].jtor_b[10], TOPP.path_info[i].jtor_b[11]);
    }
    return 0;
}

//motionflag
// int TOPP_member::compute_pathvel_by_reqvel(const int jpos_len)  //不在这里计算速度带来的约束，在后面实时轨迹规划的时候再计算，以实现调倍率
// {
//     return 0;
// }

double TOPP_member::get_min(double a, double b)
{
    double res;
    res = a;
    if (res > b)
    {
        res = b;
    }
    return res;
}

//motionflag toppuse 获取最大值
double TOPP_member::get_max(double a, double b)
{
    double res;
    res = a;
    if (res < b)
    {
        res = b;
    }
    return res;
}

void TOPP_member::init_DYN()  // for test dynamics
{
    // MOTAL_INFO(" -------------------------------- toppra_test init_DYN -------------------------------- \n");
    // double toolParameters[4] = {0.0, 0.0, 0.0, 0.0};
    // double dynP[60] = {6.7548, -1.2386, 0.1100, 0.6179, -0.0592, 6.2814, -0.2237, 0.0094, -0.0446, 0.2105,
    //               0.5324,  0.0169,-0.0166,-0.0386,  0.0175, 0.0341,  0.0081, 0.0057,  0.0010,-0.0043,
    //               0.0168,  0.0010, 0.0009, 0.0059,  0.0056, 0.0012, -0.0166, 1.4872,  3.5305,-0.0513,
    //               1.4537,  0.0402,-0.0038,-0.2254, -0.0032, 0.0505,  0.0029,-0.0013,  5.0452, 0.4753,
    //               0.4562,  0.4765,12.9006,15.1560, 10.1800, 1.9325,  1.5436, 2.1067, 14.9110, 11.9002,
    //               10.3168, 2.0228, 1.9146, 2.1963, -0.0215, 0.1246, -0.1482, 0.0015, -0.0001, -0.0060};
    // double base_offset_set[3] = {0.0, 0.0, 0.0};
    // double a[6] = {0.000000, 0.000000, 0.360381, 0.304033, 0.000000, 0.000000};
    // double d[6] = {0.120026, 0.000000, 0.000000, -0.115393, 0.113400, 0.106423};
    // double alpha[6] = {0.0, PM_PI_2, 0.0, 0.0, PM_PI_2, -PM_PI_2};
    // DynPara_init_toppra(toolParameters, base_offset_set, dynP, a, d, alpha);
}

//motionflag 设置运行模式
int RobotProxy::zucMotionSetRunMode(int mode)
{
    auto cmd = std::make_shared<CmdSetRunMode>(stat.rob_id, mode, stat.serial_num);
    int ret = mot::realtime_cmd_call(cmd);
    rcs_print("zucMotionSetRunMode: %d\n", mode);
    if (ret == 0)
    {
        if (mode == RUN_MODE_SIM_ON_REAL || mode == RUN_MODE_SIM_WITHOUT_CAN)
        {
            stat.currSimMode = 1;
        }
        else
        {
            stat.currSimMode = 0;
        }
    }
    if (stat_robotproxy.task_init_finish_flag)
    {
        // usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    return ret;
}

//motionfalg 也是一种运动模式
int RobotProxy::zucTrajRigidTap(ZucPose pos, double vel, double ini_maxvel, double acc)
{
#ifdef ISNAN_TRAP
    if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z))
    {
        printf("std::isnan error in zucTrajRigidTap()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    int id = stat.config.TrajConfig.MotionId;
    // zuc_traj_rigid_tap_t trajrigidtap = {};
    stat_robotproxy.trajrigidtap.pos.tran = pos.tran;
    stat_robotproxy.trajrigidtap.vel = vel;
    stat_robotproxy.trajrigidtap.ini_maxvel = ini_maxvel;
    stat_robotproxy.trajrigidtap.acc = acc;

    auto cmd = std::make_shared<CmdRigidTap>(stat.rob_id, id, stat_robotproxy.trajrigidtap);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucSetTrajSmooth(int trqLimitActive, int firFltActive)
{
    auto cmd = std::make_shared<CmdSetTrajsmooth>(stat.rob_id, trqLimitActive, firFltActive);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucMotionSetDebug(int debug)
{
    auto cmd = std::make_shared<CmdSetDebug>(stat.rob_id, debug);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucMotionExitProtect(int type)
{
    auto cmd = std::make_shared<CmdExitProtect>(stat.rob_id, type);
    return mot::realtime_cmd_call(cmd);
}

//safeflag 设置保护等级
int RobotProxy::zucMotionPercentageMode(int level)
{
    auto cmd = std::make_shared<CmdPercentageMode>(stat.rob_id, level);
    return mot::realtime_cmd_call(cmd);
}

//safeflag 倍率
int RobotProxy::zucMotionPercentageModeLevelValue(double percentage_mode_level1_val, double percentage_mode_level2_val)
{
    // 倍率范围 0~1
    if (percentage_mode_level1_val < 0)
        percentage_mode_level1_val = 0;
    if (percentage_mode_level1_val > 1)
        percentage_mode_level1_val = 1;
    if (percentage_mode_level2_val < 0)
        percentage_mode_level2_val = 0;
    if (percentage_mode_level2_val > 1)
        percentage_mode_level2_val = 1;

    if (percentage_mode_level2_val > percentage_mode_level1_val)  // 二级倍率 不高于一级倍率
    {
        printf("zucMotionPercentageModeLevelValue(%lf,  %lf): paramaters check [ FAILED ] \n", percentage_mode_level1_val, percentage_mode_level2_val);
        return 0;
        //percentage_mode_level2_val = percentage_mode_level1_val;
    }

    iniSavePercentageModeValue(percentage_mode_level1_val, percentage_mode_level2_val, zuc_inifile);

    auto cmd = std::make_shared<CmdSetPercentageModeLevelVal>(stat.rob_id, percentage_mode_level1_val, percentage_mode_level2_val);
    return mot::realtime_cmd_call(cmd);
}

//safeflag motionflag 设置缩减模式
int RobotProxy::zucMotReduceMode(int reduceMode)
{
    auto cmd = std::make_shared<CmdReduceMode>(stat.rob_id, reduceMode);
    return mot::realtime_cmd_call(cmd);
}

//motionflag 设置转接
int RobotProxy::zucSetupArcBlends(int arcBlendEnable,
                                  int arcBlendFallbackEnable,
                                  int arcBlendOptDepth,
                                  int arcBlendGapCycles,
                                  double arcBlendRampFreq,
                                  double arcBlendTangentKinkRatio)
{
    zuc_setup_arc_blends_t setuparcblends = {};

    setuparcblends.arcBlendEnable = arcBlendEnable;
    setuparcblends.arcBlendFallbackEnable = arcBlendFallbackEnable;
    setuparcblends.arcBlendOptDepth = arcBlendOptDepth;
    setuparcblends.arcBlendGapCycles = arcBlendGapCycles;
    setuparcblends.arcBlendRampFreq = arcBlendRampFreq;
    setuparcblends.arcBlendTangentKinkRatio = arcBlendTangentKinkRatio;

    auto cmd = std::make_shared<CmdSetupArcBlends>(stat.rob_id, setuparcblends);
    return mot::realtime_cmd_call(cmd);
}

//settgingflag  机器人设置
int RobotProxy::zucAdapt2Robot(unsigned int robotSerialNum)
{
    // int model;
    // DHParam dhParam = {};
    // RobotModelData robotModelData = {};

    // zucStatus->motion.robot_joint_num = 6;
    // // 从序列号判断机器人轴数
    // if (is7JointCobot(robotSerialNum))
    // {
    //     zucStatus->motion.robot_joint_num = 7;
    // }
    // // 设置机器人关节轴数
    // if (0 != zucTrajSetJoints(zucStatus->motion.robot_joint_num))
    // {
    //     rcs_print("zucTrajSetJoints(NUM_JOINTS) failed\n");
    //     return -1;
    // }
    // else
    // {
    //     rcs_print("zucTrajSetJoints robot joint  = %d\n", zucStatus->motion.robot_joint_num);
    // }

    // // get robot model from serial number
    // zucGetModelFromSerialNum(robotSerialNum, &model);
    // if (iniLoadRobotModelData(model, robotSerialNum, &robotModelData) != 0)
    // {
    //     zucOperatorError(UNSUPPORTED_ROBOT_MODEL, "model of connected robot is now unsupported");
    //     return -1;
    // }

    // zucStatus->motion.robot_model_data = robotModelData;
    // // printf(">>>>>>>>>>>>>>>>>>%d,  %d,  %d  \n",robotModelData.model.productNumber, robotModelData.model.productVersion, robotModelData.model.robotSeries);

    // zucSetDynamicsIdentifyTraj(robotModelData.dynamics_identify_traj_para);

    // zucSetRobotModel(model);
    // // ZucIniFile trajIniFile(ZucIniFile::ERR_TAG_NOT_FOUND | ZucIniFile::ERR_SECTION_NOT_FOUND | ZucIniFile::ERR_CONVERSION);
    // Util::IniParser ini_file(zuc_inifile);
    // if (iniLoadHistoryDHParam(&ini_file, robotSerialNum, &dhParam) != 0)
    // {
    //     dhParam = robotModelData.nominalDH;
    // }
    // if (zucmotStatus.servoDHState != 2)
    // {
    //     zucSetKineDHParams(dhParam);
    // }
    // if (zucmotStatus.servoDynState != 2)
    // {
    //     zucSetDynParams(robotModelData.dynParam);
    // }

    // zucsetJointOffsetAndDir(robotModelData.joint_offset, robotModelData.joint_dir);

    // // set motor params to motion module
    // zucMotorSetRatedCurr(robotModelData.motorRatedCurr);
    // // rcs_print("robotModelData.motorRatedCurr: %f, %f, %f\n", robotModelData.motorRatedCurr[0], robotModelData.motorRatedCurr[1], robotModelData.motorRatedCurr[2]);
    // zucMotorSetRatedTorq(robotModelData.motorRatedTorq);
    // // rcs_print("robotModelData.motorRatedTorq: %f, %f, %f\n", robotModelData.motorRatedTorq[0], robotModelData.motorRatedTorq[1], robotModelData.motorRatedTorq[2]);
    // zucMotorSetRatedTransRatio(robotModelData.motorRatedTransRatio);

    // // just follow the robot capability
    // if (robotModelData.maxTrajVel > 0)
    // {
    //     zucTrajSetMaxVelocity(robotModelData.maxTrajVel);
    //     for (int i = 0; i < 3; i++) { zucAxisSetMaxVelocity(i, robotModelData.maxTrajVel); }
    //     rcs_print("zucTrajSetMaxVelocity(robotModelData.maxTrajVel) called:%f\n", robotModelData.maxTrajVel);
    // }
    // if (robotModelData.maxTrajAcc > 0)
    // {
    //     zucTrajSetMaxAcceleration(robotModelData.maxTrajAcc);
    //     for (int i = 0; i < 3; i++) { zucAxisSetMaxAcceleration(i, robotModelData.maxTrajAcc); }
    //     rcs_print("zucTrajSetMaxAcceleration(robotModelData.maxTrajAcc) called:%f\n", robotModelData.maxTrajAcc);
    // }
    // zucAxisSetClsnAccelerationRoboModel(robotModelData.clsnTrajAcc, robotModelData.clsnTrajAcc_ori, robotModelData.clsnJointAcc);
    // if (zuc_cab_type >= 2)
    // {
    //     zucInitSafetyRobotPowerLimitSettings(zuc_inifile);
    // }
    // zucInitRobotSafetyLimitSettings(zuc_inifile);
    // zucInitRobotReduceSafetyLimitSettings(zuc_inifile);
    // // 设置机器人安全参数
    // if (0 != zucSetFunctionSaftyPara(
    //              robotModelData.robotPowerLimit, robotModelData.robotMomentumLimit, robotModelData.robotEndforceLimit, robotModelData.robotVelocityLimit))
    // {
    //     return -1;
    // }

    // for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i) { JointConfig[i].HMaxVel = robotModelData.maxJointVel[i]; }

    // // 重新active各个关节，以便6轴切7轴的时候，轴被正常激活
    // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    // {
    //     if (0 != zucJointActivate(i))
    //     {
    //         return -1;
    //     }
    // }

    // // check joint restrictions on the model
    // for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i)
    // {
    //     if (zucStatus->motion.joint[i].maxPositionLimit > robotModelData.maxJointPosLimit[i])
    //     {
    //         zucJointSetMaxPositionLimit(i, robotModelData.maxJointPosLimit[i]);
    //     }

    //     JointConfig[i].HMaxVel = robotModelData.maxJointVel[i];
    //     if (JointConfig[i].MaxVel > JointConfig[i].HMaxVel)
    //     {
    //         rcs_print("renew the max joint vel limit: %d, %f\n", i, JointConfig[i].HMaxVel);
    //         zucJointSetMaxVelocity(i, JointConfig[i].HMaxVel);
    //     }

    //     // rcs_print("joint max acc is: %f, model is: %f\n", zucStatus->motion.joint[i].maxAcc, robotModelData.maxJointAcc[i]);
    //     if (robotModelData.maxJointAcc[i] > 0)
    //     {
    //         zucJointSetMaxAcceleration(i, robotModelData.maxJointAcc[i]);
    //     }
    // }

    // zucStatus->motion.robot_serial_num = robotSerialNum;
    return 0;
}

//safeflag iniflag 更新机器人安全参数
// int RobotProxy::updateRobotSafety(int model)
// {
//     try
//     {
//         boost::property_tree::ptree m_pt, tag_setting;
//         std::string modelType = deterRobotModel(model);
//         Util::IniParser ini_file(modelType.c_str());
//         //trajInifile.Open(modelType.c_str());
//         std::string section[4] = {"NOLIMITSAFETYSETTING", "LOOSESAFETYSETTING", "MEDIUMSAFETYSETTING", "STRICTSAFETYSETTING"};
//         for (int i = 0; i < 4; ++i)
//         {
//             double momentum_limit, tcp_vel_limit, robot_power_limit, stopping_dist, stopping_time;
//             std::string node;
//             node = section[i] + ".MOMENTUM_LIMIT";
//             ini_file.get(node, momentum_limit, 70.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
//             node = section[i] + ".TCP_VEL_LIMIT";
//             ini_file.get(node, tcp_vel_limit, 1620.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
//             node = section[i] + ".ROBOT_POWER_LIMIT";
//             ini_file.get(node, robot_power_limit, 1500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
//             node = section[i] + ".STOPPING_DIST";
//             ini_file.get(node, stopping_dist, 500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
//             node = section[i] + ".STOPPING_TIME";
//             ini_file.get(node, stopping_time, 0.4, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
//             // trajInifile.Find(&x, "MOMENTUM_LIMIT", section[i].c_str());
//             // trajInifile.Find(&y, "TCP_VEL_LIMIT", section[i].c_str());
//             // trajInifile.Find(&z, "ROBOT_POWER_LIMIT", section[i].c_str());
//             // trajInifile.Find(&stopping_dist, "STOPPING_DIST", section[i].c_str());
//             // trajInifile.Find(&stopping_time, "STOPPING_TIME", section[i].c_str());

//             stat.config.safety.robotSafe[i + 2].momentumLimit = momentum_limit;
//             stat.config.safety.robotSafe[i + 2].tcpVelLimit = tcp_vel_limit;
//             stat.config.safety.robotSafe[i + 2].robotPowerLimit = robot_power_limit;
//             stat.config.safety.robotSafe[i + 2].stoppingDistance = stopping_dist;
//             stat.config.safety.robotSafe[i + 2].stoppingTime = stopping_time;
//         }

//         std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
//         read_ini(filename, m_pt);

//         if (m_pt.get_child_optional("ROBOTSAFETYSETTING"))
//         {
//             tag_setting = m_pt.get_child("ROBOTSAFETYSETTING");
//             stat.config.safety.robotSafe[0].momentumLimit = tag_setting.get<double>("MOMENTUM_LIMIT");
//             stat.config.safety.robotSafe[0].tcpVelLimit = tag_setting.get<double>("TCP_VEL_LIMIT");
//             stat.config.safety.robotSafe[0].robotPowerLimit = tag_setting.get<double>("ROBOT_POWER_LIMIT");
//             stat.config.safety.robotSafe[0].stoppingDistance = tag_setting.get<double>("STOPPING_DIST");
//             stat.config.safety.robotSafe[0].stoppingTime = tag_setting.get<double>("STOPPING_TIME");
//             stat.config.safety.robotSafe[0].custome = tag_setting.get<double>("CUSTOM_TYPE");
//             stat.config.safety.robotSafe[0].lastCustome = tag_setting.get<double>("LAST_CUSTOM_TYPE");
//             stat.config.safety.robotSafe[1].momentumLimit = tag_setting.get<double>("OLD_MOMENTUM_LIMIT");
//             stat.config.safety.robotSafe[1].tcpVelLimit = tag_setting.get<double>("OLD_TCP_VEL_LIMIT");
//             stat.config.safety.robotSafe[1].robotPowerLimit = tag_setting.get<double>("OLD_ROBOT_POWER_LIMIT");
//             stat.config.safety.robotSafe[1].stoppingDistance = tag_setting.get<double>("OLD_STOPPING_DIST");
//             stat.config.safety.robotSafe[1].stoppingTime = tag_setting.get<double>("OLD_STOPPING_TIME");

//             // 超出范围的停止时间停止距离设置为实际的生效值
//             if  (stat.config.safety.robotSafe[0].stoppingDistance > 4000.0)
//             {
//                 stat.config.safety.robotSafe[0].stoppingDistance = 4000.0;
//             }
//             else if  (stat.config.safety.robotSafe[0].stoppingDistance < 100.0)
//             {
//                 stat.config.safety.robotSafe[0].stoppingDistance = 100.0;
//             }

//             if  (stat.config.safety.robotSafe[1].stoppingDistance > 4000.0)
//             {
//                 stat.config.safety.robotSafe[1].stoppingDistance = 4000.0;
//             }
//             else if  (stat.config.safety.robotSafe[1].stoppingDistance < 100.0)
//             {
//                 stat.config.safety.robotSafe[1].stoppingDistance = 100.0;
//             }

//             if  (stat.config.safety.robotSafe[0].stoppingTime > 5)
//             {
//                 stat.config.safety.robotSafe[0].stoppingTime = 5;
//             }
//             else if  (stat.config.safety.robotSafe[0].stoppingTime < 0.1)
//             {
//                 stat.config.safety.robotSafe[0].stoppingTime = 0.1;
//             }

//             if  (stat.config.safety.robotSafe[1].stoppingTime > 5)
//             {
//                 stat.config.safety.robotSafe[1].stoppingTime = 5;
//             }
//             else if  (stat.config.safety.robotSafe[1].stoppingTime < 0.1)
//             {
//                 stat.config.safety.robotSafe[1].stoppingTime = 0.1;
//             }
//         }
//         else
//         {
//             int lastCustome = stat.config.safety.robotSafe[0].lastCustome;
//             if  (stat.config.safety.robotSafe[0].custome != lastCustome && stat.config.safety.robotSafe[0].custome != 0)
//             {
//                 lastCustome = stat.config.safety.robotSafe[0].custome;
//             }
//             stat.config.safety.robotSafe[0].momentumLimit = stat.config.safety.robotSafe[4].momentumLimit;
//             stat.config.safety.robotSafe[0].tcpVelLimit = stat.config.safety.robotSafe[4].tcpVelLimit;
//             stat.config.safety.robotSafe[0].robotPowerLimit = stat.config.safety.robotSafe[4].robotPowerLimit;
//             stat.config.safety.robotSafe[0].stoppingDistance = stat.config.safety.robotSafe[4].stoppingDistance;
//             stat.config.safety.robotSafe[0].stoppingTime = stat.config.safety.robotSafe[4].stoppingTime;
//             iniSaveRobotSafety(model,
//                                0,
//                                lastCustome,
//                                0,
//                                stat.config.safety.robotSafe[0].momentumLimit,
//                                stat.config.safety.robotSafe[0].tcpVelLimit,
//                                stat.config.safety.robotSafe[0].robotPowerLimit,
//                                stat.config.safety.robotSafe[0].stoppingDistance,
//                                stat.config.safety.robotSafe[0].stoppingTime);
//         }
//     }
//     catch (const std::exception& e)
//     {
//         printf("updateRobotSafety function error:%s\n", e.what());
//     }

//     return 0;
// }

//safeflag iniflag  更新关节安全参数
// int RobotProxy::updateJointSafety(int model, int serial)
// {
//     try
//     {
//         std::string modelType = deterRobotModel(model);
//         std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
//         Util::IniParser usrsetInifile(filename.c_str());
//         Util::IniParser trajInifile(modelType.c_str());

//         std::vector<double> jointDefaultMaxVel, jointDefaultErrLimit, jointDefaultPositiveLimit, jointDefaultNegativeLimit;
//         double jointMaxVel[MAX_AXIS_INT];
//         double jointErrLimit[MAX_AXIS_INT];
//         double jointPositiveLimit[MAX_AXIS_INT];
//         double jointNegativeLimit[MAX_AXIS_INT];

//         if (!is7JointCobot(serial))
//         {
//             if (usrsetInifile.get("JOINT.JOINT_MAX_VEL", jointDefaultMaxVel, {0, 0, 0, 0, 0, 0}, 0.0, 360.0) != Util::IniParser::ini_errcode::OK)
//             {
//                 if ((serial > 3100009 && serial <= 3109999) || (serial > 12100013 && serial <= 12139999))
//                 {
//                     trajInifile.get("JOINT.JOINT_MAX_VEL_1", jointDefaultMaxVel, {120, 120, 120, 120, 120, 120}, 0.0, 360.0);  //默认使用更严格的zu12限制
//                 }
//                 else
//                 {
//                     trajInifile.get("JOINT.JOINT_MAX_VEL", jointDefaultMaxVel, {180, 180, 180, 180, 180, 180}, 0.0, 360.0);  //默认使用zu710限制
//                 }
//             }
//             if (usrsetInifile.get("JOINT.JOINT_MAX_POS_LIMIT", jointDefaultPositiveLimit, {360.0, 265.0, 175.0, 265, 360.0, 360.0}, 0.0, 360.0) !=
//                 Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get(
//                     "JOINT.JOINT_MAX_POS_LIMIT", jointDefaultPositiveLimit, {360.0, 265.0, 175.0, 265, 360.0, 360.0}, -720, 720.0);  //默认使用zu710限制
//             }
//             if (usrsetInifile.get("JOINT.JOINT_MIN_POS_LIMIT", jointDefaultNegativeLimit, {-360.0, -85.0, -175.0, -85, -360.0, -360.0}, 0.0, 360.0) !=
//                 Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get(
//                     "JOINT.JOINT_MIN_POS_LIMIT", jointDefaultNegativeLimit, {-360.0, -85.0, -175.0, -85, -360.0, -360.0}, -720, 720.0);  //默认使用zu710限制
//             }
//             if (usrsetInifile.get("JOINT.JOINT_ERR_LIMIT", jointDefaultErrLimit, {80, 80, 80, 80, 80, 80}, 0.0, 100.0) != Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get("JOINT.JOINT_ERR_LIMIT", jointDefaultErrLimit, {80, 80, 80, 80, 80, 80}, 0, 100);  //默认使用zu710限制
//             }

//             std::string section[ZUCMOT_ROBOT_MAX_JOINTS] = {"JOINT_0", "JOINT_1", "JOINT_2", "JOINT_3", "JOINT_4", "JOINT_5", "JOINT_6"};

//             for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i)
//             {
//                 std::string s_joint_max_limit = section[i] + ".JOINT_MAX_LIMIT";
//                 std::string s_joint_min_limit = section[i] + ".JOINT_MIN_LIMIT";
//                 std::string s_joint_vel_limit = section[i] + ".JOINT_VEL_LIMIT";
//                 std::string s_joint_err_limit = section[i] + ".JOINT_ERR_LIMIT";

//                 if (usrsetInifile.get(
//                         s_joint_max_limit, jointPositiveLimit[i], jointDefaultPositiveLimit[i], jointDefaultNegativeLimit[i], jointDefaultPositiveLimit[i]) ==
//                     Util::IniParser::ini_errcode::VALUE_RANGE_ERR)
//                 {
//                     usrsetInifile.set(s_joint_max_limit, jointPositiveLimit[i]);
//                 }
//                 if (usrsetInifile.get(
//                         s_joint_min_limit, jointNegativeLimit[i], jointDefaultNegativeLimit[i], jointDefaultNegativeLimit[i], jointDefaultPositiveLimit[i]) ==
//                     Util::IniParser::ini_errcode::VALUE_RANGE_ERR)
//                 {
//                     usrsetInifile.set(s_joint_min_limit, jointNegativeLimit[i]);
//                 }
//                 usrsetInifile.get(s_joint_vel_limit, jointMaxVel[i], jointDefaultMaxVel[i], 0.0, 360.0);
//                 usrsetInifile.get(s_joint_err_limit, jointErrLimit[i], jointDefaultErrLimit[i], 0.0, 100.0);
//             }
//             syncFileToDisk(filename.c_str());
//         }
//         else
//         {
//             if (usrsetInifile.get("JOINT.JOINT_MAX_VEL", jointDefaultMaxVel, {0, 0, 0, 0, 0, 0, 0}, 0.0, 360.0) != Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get("JOINT.JOINT_MAX_VEL", jointDefaultMaxVel, {180, 180, 180, 180, 180, 180, 180}, 0.0, 360.0);  //默认使用zu710限制
//             }
//             if (usrsetInifile.get("JOINT.JOINT_MAX_POS_LIMIT", jointDefaultPositiveLimit, {360.0, 265.0, 175.0, 265, 360.0, 360.0, 360}, 0.0, 360.0) !=
//                 Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get(
//                     "JOINT.JOINT_MAX_POS_LIMIT", jointDefaultPositiveLimit, {360.0, 265.0, 175.0, 265, 360.0, 360.0, 360}, -720, 720.0);  //默认使用zu710限制
//             }
//             if (usrsetInifile.get("JOINT.JOINT_MIN_POS_LIMIT", jointDefaultNegativeLimit, {-360.0, -85.0, -175.0, -85, -360.0, -360.0, -360}, 0.0, 360.0) !=
//                 Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get("JOINT.JOINT_MIN_POS_LIMIT",
//                                 jointDefaultNegativeLimit,
//                                 {-360.0, -85.0, -175.0, -85, -360.0, -360.0, -360},
//                                 -720,
//                                 720.0);  //默认使用zu710限制
//             }
//             if (usrsetInifile.get("JOINT.JOINT_ERR_LIMIT", jointDefaultErrLimit, {80, 80, 80, 80, 80, 80, 80}, 0.0, 100.0) != Util::IniParser::ini_errcode::OK)
//             {
//                 trajInifile.get("JOINT.JOINT_ERR_LIMIT", jointDefaultErrLimit, {80, 80, 80, 80, 80, 80, 80}, 0, 100);  //默认使用zu710限制
//             }

//             std::string section[ZUCMOT_ROBOT_MAX_JOINTS] = {"JOINT_0", "JOINT_1", "JOINT_2", "JOINT_3", "JOINT_4", "JOINT_5", "JOINT_6"};

//             for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i)
//             {
//                 std::string s_joint_max_limit = section[i] + ".JOINT_MAX_LIMIT";
//                 std::string s_joint_min_limit = section[i] + ".JOINT_MIN_LIMIT";
//                 std::string s_joint_vel_limit = section[i] + ".JOINT_VEL_LIMIT";
//                 std::string s_joint_err_limit = section[i] + ".JOINT_ERR_LIMIT";

//                 if (usrsetInifile.get(
//                         s_joint_max_limit, jointPositiveLimit[i], jointDefaultPositiveLimit[i], jointDefaultNegativeLimit[i], jointDefaultPositiveLimit[i]) ==
//                     Util::IniParser::ini_errcode::VALUE_RANGE_ERR)
//                 {
//                     usrsetInifile.set(s_joint_max_limit, jointPositiveLimit[i]);
//                 }
//                 if (usrsetInifile.get(
//                         s_joint_min_limit, jointNegativeLimit[i], jointDefaultNegativeLimit[i], jointDefaultNegativeLimit[i], jointDefaultPositiveLimit[i]) ==
//                     Util::IniParser::ini_errcode::VALUE_RANGE_ERR)
//                 {
//                     usrsetInifile.set(s_joint_min_limit, jointNegativeLimit[i]);
//                 }
//                 usrsetInifile.get(s_joint_vel_limit, jointMaxVel[i], jointDefaultMaxVel[i], 0.0, 360.0);
//                 usrsetInifile.get(s_joint_err_limit, jointErrLimit[i], jointDefaultErrLimit[i], 0.0, 100.0);
//             }
//             syncFileToDisk(filename.c_str());
//         }

//         for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i)
//         {
//             zucStatus->jointLimit[i].jointVelLimit = jointMaxVel[i];
//             zucStatus->jointLimit[i].jointErrLimit = jointErrLimit[i];
//             zucStatus->jointLimit[i].posLimt.positive_limit = jointPositiveLimit[i];
//             zucStatus->jointLimit[i].posLimt.negative_limit = jointNegativeLimit[i];
//             zucStatus->jointLimit[i].posLimt.joint_id = i;
//             zucStatus->jointLimit[i + zucStatus->motion.robot_joint_num].jointVelLimit = jointDefaultMaxVel[i];
//             zucStatus->jointLimit[i + zucStatus->motion.robot_joint_num].jointErrLimit = jointDefaultErrLimit[i];
//             zucStatus->jointLimit[i + zucStatus->motion.robot_joint_num].posLimt.positive_limit = jointDefaultPositiveLimit[i];
//             zucStatus->jointLimit[i + zucStatus->motion.robot_joint_num].posLimt.negative_limit = jointDefaultNegativeLimit[i];
//             zucStatus->jointLimit[i + zucStatus->motion.robot_joint_num].posLimt.joint_id = i;
//         }
//     }
//     catch (const std::exception& e)
//     {
//         printf("updateRobotSafety function error:%s\n", e.what());
//     }

//     return 0;
// }

//safeflag iniflag  更新关节安全参数
// int RobotProxy::iniSaveJointSafety(int joint, double value, ZucJointSafetyType type)
// {
//     try
//     {
//         boost::property_tree::ptree m_pt, tag_setting;
//         std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
//         read_ini(filename, m_pt);

//         char tmp[30];
//         sprintf(tmp, "%3.3f", value);
//         std::stringstream st;
//         st << joint;
//         std::string item;
//         switch (type)
//         {
//         case ZucJointSafetyType::JOINT_VEL_LIMIT:
//             item = "JOINT_" + st.str() + ".JOINT_VEL_LIMIT";
//             break;
//         case ZucJointSafetyType::JOINT_ERR_LIMIT:
//             item = "JOINT_" + st.str() + ".JOINT_ERR_LIMIT";
//             break;
//         }

//         m_pt.put<std::string>(item, tmp);
//         boost::property_tree::ini_parser::write_ini(filename, m_pt);
//         syncFileToDisk(filename.c_str());

//         return 0;
//     }
//     catch (const std::exception& e)
//     {
//         printf("iniSaveJointSafety function error:%s\n", e.what());
//         return -1;
//     }
// }

//safeflag iniflag  设置机器人安全参数
int RobotProxy::zucSetRobotSafety(ZUC_MOTION_STAT* stat, ZUC_ROBOT_SAFETY robot_safety)
{
    int model;
    // get robot model from serial number
    // zucGetModelFromSerialNum(zucStatus->motion.robot_serial_num, &model);

    // // if (0 != checkRobotSafetyParameters(&robot_safety)) // disableflag 先注释 ybh need fix
    // // {
    // //     stat_robotproxy.usrOpeErrCode = -1;
    // //     return -1;
    // // }

    // int lastCustome = stat.config.safety.robotSafe[0].lastCustome;
    // if (stat.config.safety.robotSafe[0].custome != lastCustome && stat.config.safety.robotSafe[0].custome != robot_safety.custome)
    // {
    //     lastCustome = stat.config.safety.robotSafe[0].custome;
    // }
    // iniSaveRobotSafety(model,
    //                    robot_safety.custome,
    //                    lastCustome,
    //                    robot_safety.paramType,
    //                    robot_safety.momentumLimit,
    //                    robot_safety.tcpVelLimit,
    //                    robot_safety.robotPowerLimit,
    //                    robot_safety.stoppingDistance,
    //                    robot_safety.stoppingTime);
    // zucGetRobotSafety(stat);
    // if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_TCPVEL_PARA)
    //     zucSetRobotTcpVelLimit(robot_safety.tcpVelLimit);
    // if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_MOMENTUM_PARA)
    //     zucSetRobotMomentumLimit(robot_safety.momentumLimit);
    // if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_POWER_PARA)
    //     zucSetRobotPowerLimit(robot_safety.robotPowerLimit, 1);
    // if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_STOPPINGDIST_PARA)
    //     zucSetRobotStoppingDist(robot_safety.stoppingDistance);
    // if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_STOPPINGTIME_PARA)
    //     zucSetRobotStoppingTime(robot_safety.stoppingTime);
    // stat_robotproxy.usrOpeErrCode = 0;
    return 0;
}

//motionflag  7轴逆接模式
int RobotProxy::zucSetRobot7dof_invflag(int inv_flag)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofInvFlag>(stat.rob_id, inv_flag);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

//motionflag  设置全参数补偿 同时也会切到数值解
int RobotProxy::zucSetRobotfull_dh_flag(int full_dh_flag)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobotFullDhFlag>(stat.rob_id, full_dh_flag);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

//motionflag  设置7轴臂角
int RobotProxy::zucSetRobot7dof_arm_angle(double arm_angle)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofArmAngle>(stat.rob_id, arm_angle);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

//motionflag  设置7轴逆接模式
int RobotProxy::SetRobot7dofinv_inform(int inv_flag, double arm_angle)
{
    zucSetRobot7dof_invflag(inv_flag);
    zucSetRobot7dof_arm_angle(arm_angle);
    return 0;
}

//motionflag  设置全参数补偿 同时也会切到数值解
int RobotProxy::SetRobotFull_DH_flag(int full_dh_flag)
{
    zucSetRobotfull_dh_flag(full_dh_flag);
    zucStatus->motion.full_dh_com = full_dh_flag;
    return 0;
}

//motionflag  设置7轴避限位逆接参数
int RobotProxy::SetRobot7dof_kine_param_alpha_k(double k, double alpha)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofKineParamAlphaK>(stat.rob_id, k, alpha);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
    return 0;
}

//safeflag  设置缩减安全参数
int RobotProxy::zucSetReduceSafety(ZUC_MOTION_STAT* stat, reduce_param_config reduce_param)
{
    // int model;
    // // get robot model from serial number
    // // zucGetModelFromSerialNum(stat->robot_serial_num, &model);
    // zucGetModelFromSerialNum(zucStatus->motion.robot_serial_num, &model);
    // // Todo
    // // if (0 != checkReduceSafetyParameters(&reduce_param)) //disableflag
    // // {
    // //     return -1;
    // // }
    // iniSaveReduceSafety(0, reduce_param);
    // stat.config.safety.reduceSafe.tcp_speed = reduce_param.tcp_vel_reduce;
    // stat.config.safety.reduceSafe.elbow_speed = reduce_param.elbow_vel_limit_reduce;
    // stat.config.safety.reduceSafe.momentum_limit = reduce_param.robot_momentum_limit_reduce;
    // stat.config.safety.reduceSafe.power_limit = reduce_param.robot_poewr_limit_reduce;
    // stat.config.safety.reduceSafe.stopping_dist = reduce_param.stopping_dist_reduce;
    // stat.config.safety.reduceSafe.stopping_time = reduce_param.stopping_time_reduce;
    // zucSetRobotElbowVelLimitReduce(reduce_param.elbow_vel_limit_reduce);
    // zucSetRobotTcpVelLimitReduce(reduce_param.tcp_vel_reduce);
    // zucSetRobotMomentumLimitReduce(reduce_param.robot_momentum_limit_reduce);
    // zucSetRobotPowerLimitReduce(reduce_param.robot_poewr_limit_reduce);
    // zucSetRobotStoppingDistReduce(reduce_param.stopping_dist_reduce);
    // zucSetRobotStoppingTimeReduce(reduce_param.stopping_time_reduce);
    return 0;
}

//safeflag  motionflag  拖拽TCP速度限制
int RobotProxy::zucSetDragTcpSpeedLimitSetting(double dragTcpSpeedLimit)
{
    //伟创力定制的手自动模式功能开启，且在自动模式下，禁止设置安全参数 || 正在运动中则禁止设置安全参数
    if ((stat.operation_mode_option == 1 && stat.operation_mode == 1) || stat.motion.inpos != 1)
    {
        zucOperatorError(NONMANUAL_CANNOT_SET_ROBOT_SAFETY_PARAMETERS,
                         "The safety parameters cannot be configured if the robot is not in manual mode or in manual mode motion.");
        return -1;
    }
    //如果程序执行则禁止设置安全参数
    if (stat_robotproxy.interpState == ZUC_TASK_INTERP_READING || stat_robotproxy.interpState == ZUC_TASK_INTERP_PAUSED ||
        stat_robotproxy.interpState == ZUC_TASK_INTERP_WAITING)
    {
        zucOperatorError(PROGRAM_EXECUTE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the program is being executed.");
        return -1;
    }
    // if (zucStatus->motion.is_enabled)
    // {
    //     zucOperatorError(ENABLE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the robot is enable.");
    //     return -1;
    // }
    if ((dragTcpSpeedLimit - 50.0) < -0.0001)
    {
        dragTcpSpeedLimit = 50;
    }
    if (dragTcpSpeedLimit - 1500.0 > 0.0001)
    {
        dragTcpSpeedLimit = 1500;
    }
    if (0 != iniSaveDragSpeedLimit(dragTcpSpeedLimit))
    {
        return -1;
    }
    if (0 != zucSetDragTcpSpeedLimit(dragTcpSpeedLimit))
    {
        return -1;
    }
    return 0;
}

//safeflag  motionflag 设置TCP速度限制
int RobotProxy::zucSetRobotTcpVelLimit(float tcpVelLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetTcpVelLimit>(stat.rob_id, tcpVelLimit);
        ret = mot::realtime_cmd_call(cmd);

        TOPP.tcp_vel_limit_toppra = tcpVelLimit;
        if (stat_robotproxy.task_init_finish_flag)
        {
            // usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotTcpVelLimit function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag 设置停止距离
int RobotProxy::zucSetRobotStoppingDist(float stoppingDist)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingDist>(stat.rob_id, stoppingDist);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotstoppingDist function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag 设置停止时间
int RobotProxy::zucSetRobotStoppingTime(float stoppingTime)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingTime>(stat.rob_id, stoppingTime);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag 设置肘部位置限制
int RobotProxy::zucSetRobotElbowVelLimitReduce(double elbowVelLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetElbowVelLimitReduce>(stat.rob_id, elbowVelLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag 设置拖拽tcp速度限制
int RobotProxy::zucSetDragTcpSpeedLimit(double dragTcpSpeedLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetDragTcpSpeedLimit>(stat.rob_id, dragTcpSpeedLimit);
        return mot::realtime_cmd_call(cmd);
        if (stat_robotproxy.task_init_finish_flag)
        {
            // usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetDragTcpSpeedLimit function error:%s\n", e.what());
        return -1;
    }
    return 0;
}

//safeflag  motionflag 设置缩减tcp速度限制
int RobotProxy::zucSetRobotTcpVelLimitReduce(double tcpVelLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetTcpVelLimitReduce>(stat.rob_id, tcpVelLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag
int RobotProxy::zucSetRobotMomentumLimitReduce(double momentumLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetMomentumLimitReduce>(stat.rob_id, momentumLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag
int RobotProxy::zucSetRobotPowerLimitReduce(double robotPowerLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobotPowerLimitReduce>(stat.rob_id, robotPowerLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag
int RobotProxy::zucSetRobotStoppingDistReduce(double stopping_dist_reduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingDistReduce>(stat.rob_id, stopping_dist_reduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag
int RobotProxy::zucSetRobotStoppingTimeReduce(double stopping_time_reduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingTimeReduce>(stat.rob_id, stopping_time_reduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

//safeflag  motionflag
int RobotProxy::zucSetRobotMomentumLimit(float momentumLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetMomentumLimit>(stat.rob_id, momentumLimit);
        return mot::realtime_cmd_call(cmd);
        if (stat_robotproxy.task_init_finish_flag)
        {
            // usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotMomentumLimit function error:%s\n", e.what());
        return -1;
    }
}

// int RobotProxy::zucSetOperationMode(unsigned int mode)
// {
//     try
//     {
//         std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
//         Util::IniParser ini_file(filename.c_str());
//         ini_file.set("OPERATIONMODE.MODE", std::to_string(mode));
//         //ini_putl("OPERATIONMODE", "MODE", mode, filename.c_str());
//         syncFileToDisk(filename.c_str());
//         auto cmd = std::make_shared<CmdSetOperationmode> (stat.rob_id, mode);
//         return mot::realtime_cmd_call (stat.rob_id, cmd);
//     }
//     catch (const std::exception& e)
//     {
//         printf("ZUCMOT_SET_OPERATIONMODE function error:%s\n", e.what());
//         return -1;
//     }
// }

int RobotProxy::zucSetOperationModeOption(unsigned int enable)
{
    try
    {
        auto cmd = std::make_shared<CmdSetOperationmodeOption>(stat.rob_id, enable);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("ZUCMOT_SET_OPERATIONMODE_OPTION function error:%s\n", e.what());
        return -1;
    }
}

int RobotProxy::do_movj(RobJointVal jpos, MoveSpeed speed, MovjParam movj_param)
{
    int ret = 0;
    if (1)  // #FIXME  临时使用jog_movj代替movj
    {
        double posCmd[7];
        jpos.array_value(posCmd);
        int end_cond[3];
        end_cond[0] = movj_param.di_type;
        end_cond[1] = movj_param.di_index;
        end_cond[2] = movj_param.di_state;
        ret = zucJogMovj(
            movj_param.jog_movj.mvOpt,
            posCmd,
            speed.j_vel,
            speed.j_acc,
            movj_param.jog_movj.tol,
            movj_param.jog_movj.executing_line_id,
            end_cond,
            movj_param.jog_movj.is_cpos);  //motionflag SDK jog 应该是SDK有增量和绝对值两种模式，而解析里只有直线运动，要是想合并的话应该要把计算挪到一起
    }
    else
    {
        ZucPose end;
        memset(&end, 0, sizeof(ZucPose));
        int type = movj_param.type;
        double ini_maxvel = movj_param.ini_maxvel;
        double acc = speed.j_acc;
        double jerk = speed.j_jerk;
        int indexrotary = movj_param.indexrotary;
        int di_type = movj_param.di_type;
        int di_index = movj_param.di_index;
        int di_state = movj_param.di_state;
        int jointNum = stat.robot_joint_num;
        int planner_type = movj_param.planner_type;
        ret = zucTrajJointMove(end, type, speed.j_vel, ini_maxvel, speed.j_acc, speed.j_jerk, indexrotary, di_type, di_index, di_state, jointNum, planner_type);
    }

    return ret;
};
int RobotProxy::do_movl(RobTarget target, MoveSpeed speed, MovLParam movl_param, int tool_id, int usrframe_id)
{
    int ret = 0;
    if (1)  // #FIXME  临时使用jog_movl代替movl
    {
        int end_cond[3];
        end_cond[0] = movl_param.di_type;
        end_cond[1] = movl_param.di_index;
        end_cond[2] = movl_param.di_state;
        ret = zucJogMovl(movl_param.jog_movl.mvOpt,
                         target.pose,
                         speed.vel,
                         speed.acc,
                         speed.ori_vel,
                         speed.ori_acc,
                         movl_param.jog_movl.tol,
                         movl_param.jog_movl.executing_line_id,
                         end_cond);  //motionflag SDK直线 与关节是同样的原因
    }
    else
    {
        int type = movl_param.type;
        double ini_maxvel = movl_param.ini_maxvel;
        int indexrotary = movl_param.indexrotary;
        int using_abc = movl_param.using_abc;
        int di_type = movl_param.di_type;
        int di_index = movl_param.di_index;
        int di_state = movl_param.di_state;
        int planner_type = movl_param.planner_type;
        ret = zucTrajLinearMove(target.pose,
                                type,
                                speed.vel,
                                ini_maxvel,
                                speed.acc,
                                speed.jerk,
                                speed.ori_vel,
                                speed.ori_acc,
                                indexrotary,
                                using_abc,
                                di_type,
                                di_index,
                                di_state,
                                planner_type);
    }
    return ret;
};
int RobotProxy::do_movc(RobTarget target, RobTarget mid, MoveSpeed speed, Movc_Param movc_param, int tool_id, int usrframe_id)
{
    double ini_maxvel = 0;
    int using_abc = 0;
    double circle_count = 0;
    int di_type = 0;
    int di_index = 0;
    int di_state = 0;
    int planner_type = 0;
    int circlemode = 0;
    int ret = zucTrajMovc(
        mid.pose, target.pose, speed.vel, ini_maxvel, speed.acc, speed.jerk, using_abc, circle_count, di_type, di_index, di_state, planner_type, circlemode);
    return ret;
    // return 0;
};
int RobotProxy::do_movs(Movs_Param movs_param)
{
    int ret = zucTrajMovs(movs_param.a0, movs_param.a1, movs_param.a2, movs_param.a3, movs_param.time);
    return ret;
};

int RobotProxy::zucJogMovl(int mvOpt,
                           const ZucPose& posCmd,
                           double vel,
                           double acc,
                           double orivel,
                           double oriacc,
                           double tol,
                           int executing_line_id,
                           int* end_cond)  //motionflag SDK直线 与关节是同样的原因
{
    // @FIXME 这些参数从zucmotcommand中删除了，会影响toppa
    zuc_jog_t jog = {};
    jog.cartPose = posCmd;
    jog.tol = tol;
    jog.executing_line_id = executing_line_id;
    jog.vel = vel;
    jog.acc = acc;
    jog.ori_vel = orivel;
    jog.ori_acc = oriacc;
    for (int i = 0; i < 3; i++) { jog.end_cond[i] = end_cond[i]; }
    jog.moveOpt = ((mvOpt >> 4) & 0xF);  // 1 for jog mode
    CmdJogMovl::JogMovlType type;
    if ((mvOpt & 0xF) == 1)
    {
        type = CmdJogMovl::JogMovlType::JOG_MOVL_INCR;
    }
    else if ((mvOpt & 0xF) == 2)
    {
        type = CmdJogMovl::JogMovlType::JOG_MOVL_TOOL_INCR;
    }
    else
    {
        type = CmdJogMovl::JogMovlType::JOG_MOVL_ABS;
    }
    auto cmd = std::make_shared<CmdJogMovl>(stat.rob_id, type, jog);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::zucJogMovj(int mvOpt,
                           double* posCmd,
                           double vel,
                           double acc,
                           double tol,
                           int executing_line_id,
                           int* end_cond,
                           int is_cpos)  //motionflag SDK jog 应该是SDK有增量和绝对值两种模式，而解析里只有直线运动，要是想合并的话应该要把计算挪到一起
{
    zuc_jog_t jog = {};
    int jog_type = -1;
    for (int i = 0; i < stat.robot_joint_num; ++i) { jog.posCmd[i] = posCmd[i]; }

    jog.vel = vel;
    jog.acc = acc;
    jog.tol = tol;
    jog.executing_line_id = executing_line_id;
    for (int i = 0; i < 3; i++) { jog.end_cond[i] = end_cond[i]; }
    jog.moveOpt = ((mvOpt >> 4) & 0xF);  // 1 for jog mode
    jog.is_cpos = is_cpos;               // ABS OR INCR
    if (mvOpt & 0xF) {}
    else
    {
        for (int i = 0; i < stat.robot_joint_num; i++)
        {
            TOPP.last_tc_goal_jointpos[i] = jog.posCmd[i];
            TOPP.last_tc_blend_jointpos[i] = jog.posCmd[i];
            for (int j = 0; j < REFIT_NUM; j++) { TOPP.last_tc_refit_jpos[j][i] = jog.posCmd[i]; }
            TOPP.last_tc_blend_r = 0.0;
            TOPP.last_final_pathp = 0.0;
            TOPP.lasttc_jointNum = 0.0;
        }
    }
    auto cmd = std::make_shared<CmdJogMovj>(stat.rob_id, jog);
    return mot::realtime_cmd_call(cmd);
}

int RobotProxy::do_stop()
{
    int ret = zucTrajAbort();
    return ret;
};